
//
// Created by sukai on 4/1/23.
//

#include "moveAction.h"
using namespace contnav_moveAction;
namespace contnav_moveAction {
    moveAction::moveAction(tf2_ros::Buffer& tf,ros::NodeHandle &nh): tf_(tf),nh_(nh), state_machine_(nh)

                                                         //as_(NULL),
                                                         //点到点     GotoCtrl = actionlib::SimpleActionClient<mbf_msgs::MoveBaseAction>;//点到点
                                                         //goto_ctrl_(new GotoCtrl("move_base_flex/move_base")),
                                                          //给路径直接走   ExeCtrl = actionlib::SimpleActionClient<mbf_msgs::ExePathAction>;
                                                         //exe_ctrl_(new ExeCtrl("move_base_flex/exe_path")),
                                                         //ac_("/move_base_flex/exe_path", true)//ExeCtrl

                                                         {





        //状态机
        state_machine_.add_state("navigate_to_goalpath", std::bind(&moveAction::navigate_to_goalpath, this, std::placeholders::_1));//加入状态 navigate_to_goalpath 导航状态
        state_machine_.add_state("navigate_to_goalpathAgain", std::bind(&moveAction::navigate_to_goalpath, this, std::placeholders::_1));//加入状态 navigate_to_goalpathAgain 导航状态重复发布导航

        state_machine_.add_state("navigate_to_goalpathDefaultgoal", std::bind(&moveAction::navigate_to_goalpathDefaultgoal, this, std::placeholders::_1));//加入状态 navigate_to_goalpathDefaultgoal 导航到默认位置
        state_machine_.add_state("nnavigate_to_goalpathDefaultgoalAgain", std::bind(&moveAction::navigate_to_goalpathDefaultgoal, this, std::placeholders::_1));//加入状态 navigate_to_goalpathDefaultgoal 导航到默认位置


        state_machine_.add_state("navigate_to_goalpathOverturn", std::bind(&moveAction::navigate_to_goalpathOverturn, this, std::placeholders::_1));//加入状态 navigate_to_goalpathAgain 导航状态重复发布导航
        state_machine_.add_state("navigate_to_goalpathOverturnAgain", std::bind(&moveAction::navigate_to_goalpathOverturn, this, std::placeholders::_1));//加入状态 navigate_to_goalpathOverturnAgain 导航状态重复发布导航

        state_machine_.add_state("charging", std::bind(&moveAction::chargeFun, this, std::placeholders::_1));//给充电桩发送充电指令
        state_machine_.add_state("monitor_progress", std::bind(&moveAction::monitor_progress, this, std::placeholders::_1));//加入状态 monitor_progress 监控进度状态
     // state_machine_.add_state("monitor_progress", std::bind(&moveAction::monitor_progress, this, std::placeholders::_1));//加入状态 monitor_progress 监控进度状态
        //初始化给一个控制
        geometry_msgs::PoseStamped start_pose,target_pose,target_pose_default;
        state_machine_.set_state("idle", {start_pose,target_pose,target_pose_default,std::string("EMPTY")});//闲置状态,初始化给一个控制
//--------------------------------------------------------------------------------------------------



//---------------------------------------------------------------------------------------------------
    }

    moveAction::~moveAction() {}




    //日志级别 日志内容
    bool moveAction::saveLogFormat(string logLevel, string className, int classLine, char *format, ...) {

        string logcontent = "";
        va_list arg_ptr;
        va_start(arg_ptr, format);
        int len = vsnprintf(NULL, 0, format, arg_ptr);
        va_end(arg_ptr);
        if (len > 0) {
            char *content = new char[len + 1];
            va_start(arg_ptr, format);
            vsnprintf(content, len + 1, format, arg_ptr);
            va_end(arg_ptr);
            content[len] = 0;
            logcontent = content;
            delete[] content; //动态的数组需要删除
            // m_len += len;//获取到当前日志的长度 累计起来
        }
        cout << "logcontent: " << logcontent << endl;

        //查询字典数据
       // contnavLoggerClient.waitForExistence();
        contnav_srvs::ContnavLogger contnavLoggerService;
        contnavLoggerService.request.request_type = ContnavLoggerType::savelog;// 保存日志   ContnavLoggerType::savelog:保存日志||   ContnavLoggerType::deleteAlllog:删除所有日志文件  ||  ContnavLoggerType::showlogSize 查看日志大小
        contnavLoggerService.request.logFileName = logFileName;// 日志文件名称
        contnavLoggerService.request.className = className;// 代码file名称 [注意： 确保每个服务节点只使用一个日志文件名,日志服务节点是根据文件名来创建对应的日志对象，请确保一个服务对应一个日志文件名称]
        contnavLoggerService.request.classLine = classLine;// 代码行数据
        contnavLoggerService.request.logLevel = logLevel;//日志类型,默认 debug
        contnavLoggerService.request.logcontent = logcontent;//日志内容

        //发送seervice
        contnavLoggerClient.call(contnavLoggerService);

        if (contnavLoggerService.response.result != "ok") {

            cout << "close ros  test.cppcontnavLoggerService.response.message: " << contnavLoggerService.response.message
                 << endl;
            return false;
        }
        return true;


    }

//sql 查询字典数据
    void moveAction::queryAllFromTABLE_DICTIONARES() {
        try {

            std::cout << "==1================moveAction 查询字典数据===================" << std::endl;
            //查询字典数据
            SqlOperationClient.waitForExistence();
            contnav_srvs::TABLE_CONTORLLER tableContorllerService;
            tableContorllerService.request.request_type = TabeOperationType::selectSQLTABL;//selectSQLTABL：查询数据操作
            tableContorllerService.request.request_where_dictionariesVector.request_type = TabeName::TABLE_DICTIONARES;//要操作的数据库表
            tableContorllerService.request.isReturn_result_sqlStr = "true";//需要返回sql语句

            /**
               * 打开注释为按条件查询
           //1. where 条件
           whereSql("","Id","=","1",tableContorllerService);
          */
            //发送seervice
            SqlOperationClient.call(tableContorllerService);

            if (tableContorllerService.response.result != "ok")
                return;

            for (int l = 0; l < tableContorllerService.response.result_table_dictionares.size(); ++l) {
                contnav_msgs::TABLE_DICTIONARES table_dictionares = tableContorllerService.response.result_table_dictionares[l];
                dictionariesMap_[table_dictionares.Key_i] = table_dictionares.Value;
                cout << "Id: " << table_dictionares.Id << ", ";
                cout << "TargetName: " << table_dictionares.TargetName << ", ";
                cout << "Key_i: " << table_dictionares.Key_i << ", ";
                cout << "Value: " << table_dictionares.Value << ", ";
                cout << "Status: " << table_dictionares.Status << "; " << endl;

            }
            /*
            //遍历map,查看数据
            map<string, string>::iterator iter;
            for(iter = dictionariesMap_.begin(); iter != dictionariesMap_.end(); iter++) {
                cout << iter->first << " : " << iter->second << endl;
            }
           */
        } catch (exception e) {
            cout << e.what() << endl;
        }
    }

//sql where 条件
    void moveAction::whereSql(std::string Condition, std::string Key_i, std::string Symbol, std::string Value,
                                  contnav_srvs::TABLE_CONTORLLER &tableContorllerService) {

        //1. where 条件
        contnav_msgs::Dictionaries dictionaries;
        dictionaries.Condition = Condition;
        dictionaries.Key_i = Key_i;
        dictionaries.Symbol = Symbol;
        dictionaries.Value = Value;
        tableContorllerService.request.request_where_dictionariesVector.DictionariesVectors.push_back(dictionaries);

    }


//sql封装条件数据
    void moveAction::mapTOcontnav_srvs(map <string, string> &dictionarieMap_request_value,
                                           contnav_srvs::TABLE_CONTORLLER &tableContorllerService) {

        map<string, string>::iterator iter_value;
        for (iter_value = dictionarieMap_request_value.begin();
             iter_value != dictionarieMap_request_value.end(); iter_value++) {
            contnav_msgs::Dictionaries dictionaries;
            dictionaries.Key_i = iter_value->first;
            dictionaries.Value = iter_value->second;
            //  cout << " =========type==\"value\" || type==\"all\"======="  << endl;

            cout << iter_value->first << " : " << iter_value->second << endl;
            tableContorllerService.request.request_value_dictionariesVector.DictionariesVectors.push_back(dictionaries);
        }

    }



    //多线程 处理状态机
    void moveAction::thread_msg() {
        ros::Rate r(10);
        while (ros::ok()) {
            state_machine_.execute();
            ros::spinOnce();
            // ros::spinOnce();
            //std::this_thread::sleep_for(std::chrono::milliseconds(100));
            r.sleep();
        }
    }

    //多线程 发布速度停止指令
    void moveAction::stopCmd_thread() {
        //发布速度
        geometry_msgs::Twist cmd_vel;
        cmd_vel.linear.x = 0;
        cmd_vel.linear.y = 0;
        cmd_vel.linear.z = 0;
        cmd_vel.angular.x = 0;
        cmd_vel.angular.y = 0;
        cmd_vel.angular.z = 0;
        ros::Rate r(5);
        while (stopCmdFig_) {
            cmd_vel_pub_.publish(cmd_vel);
            ros::spinOnce();
            r.sleep();
        }
    }


//刷新数据
    void moveAction::callBack_refreshdb(std_msgs::String data) {
        try {
            std::cout << "====刷新数据===moveAction====callBack_refreshdb===data.data====" << std::endl;
            queryAllFromTABLE_DICTIONARES();
        } catch (std::exception e) {
            e.what();
        }
    }

//定时执行
    void moveAction::updatefun(const ros::TimerEvent &time_event) {
        try {

            timer2_.stop();
            //查询字典数据
            queryAllFromTABLE_DICTIONARES();


            std::cout << "============1=====定时执行=moveAction::updatefun===================" << std::endl;
        } catch (exception e) {
            cout << e.what() << endl;
        }
    }


    //service
    bool moveAction::callback_global_plannerFun(contnav_srvs::moveAction::Request &request,
                                                    contnav_srvs::moveAction::Response &response) {


        std::cout << "1.路径规划 moveAction================================================== " << std::endl;

        //互锁
       // m_.lock();
        //使用： 自动解锁
        AutoLock lock(&m_mutex);
        bool fig = true;
        //恢复代价地图
        std_srvs::Empty srv;
        clear_costmaps_client_.call(srv);
        try {
           if(request.request_type=="moveAction") {
               restore();//恢复
               //查询是否存在半结构化路径规划
               contnav_srvs::global_planner global_planner_srv;
               global_planner_srv.request.request_type = "getExecuteMapPlanSize";
               //通过service获取路径
               my_global_plannerClient.call(global_planner_srv);
               if(global_planner_srv.response.result!=ServiceResultType::ResultTypeOK){
                   ROS_INFO("callback_global_plannerFun::moveAction 获取路径失败,请先规划半结构化路径  frame_id_ %s ",frame_id_.c_str());
                   response.message="errror callback_global_plannerFun::moveAction 获取路径失败,请先规划半结构化路径frame_id_: %s "+frame_id_;
                   response.result = ServiceResultType::error;
                   return true;
               }


            //todo (暂时写死)返回默认点位，或是充电点位
            request.request_default_goal.header.stamp = ros::Time::now();
            request.request_default_goal.header.frame_id = frame_id_;
            request.request_default_goal.pose.position.x = -4.4;
            request.request_default_goal.pose.position.y = 1.11;
            request.request_default_goal.pose.position.z = 0.0;
            request.request_default_goal.pose.orientation.x = 0.0;
            request.request_default_goal.pose.orientation.y = 0.0;
            request.request_default_goal.pose.orientation.z = 0.0;
            request.request_default_goal.pose.orientation.w = 1.0;

            if(request.request_default_goal.header.frame_id==""){
                string meg="request.request_default_goal.header.frame_id==map";
                ROS_INFO("默认点位，或是充电点位 必须给值 request.request_default_goal  frame_id_ %s ",frame_id_.c_str());
                response.message="errror 默认点位，或是充电点位 必须给值 request.request_default_goal frame_id_: %s "+frame_id_;
                response.result = ServiceResultType::error;
                //日志
                contnav_error(__FILE__,__LINE__,const_cast<char*>(response.message.c_str()));
                return true;
            }

            //起始点坐标
            if(request.request_start.header.frame_id.empty())
            {
                geometry_msgs::PoseStamped start_pose;
                if(!getRobotPose(start_pose)){
                    response.result = ServiceResultType::error;
                    ROS_ERROR("move_base cannot make a plan for you because it could not get the start pose of the robot");
                    response.message=" error 路径规划 makePlan getRobotPose 未获取到机器人当前位置！ ";
                    //m_.unlock();
                    //日志
                    //  contnav_error(__FILE__,__LINE__,const_cast<char*>(response.message.c_str()));
                    //  contnav_error(__FILE__,__LINE__,"text");
                    return true;
                }
                request.request_start = start_pose;
            }

            std::vector<geometry_msgs::PoseStamped> plan;
            //目标坐标
            request.request_goal.header.stamp = ros::Time::now();
            request.request_goal.header.frame_id = frame_id_;
            // todo 测试数据
              // request.request_goal.pose.position.x = 0.814;
             //  request.request_goal.pose.position.y = 3.66;
            request.request_goal.pose.position.x = -1.28;
            request.request_goal.pose.position.y = 1.53;
            request.request_goal.pose.position.z = 0.0;
            request.request_goal.pose.orientation.x = 0.0;
            request.request_goal.pose.orientation.y = 0.0;
            request.request_goal.pose.orientation.z = 0.0;
            request.request_goal.pose.orientation.w = 1.0;

            isReturnPile_=request.isReturnPile;//超过时间： 是否返回充电桩 false绕障碍,rue回桩 -> state_machine_.set_state("navigate_to_goalpathDefaultgoal"
           // isSuspend_=request.isSuspend; //true:暂停,false 绕障碍

              isSendGoalPlanOverturn_=request.isSendGoalPlanOverturn;//（直接反向规划）是否反转路径导航，true 反转路径导航，false 正常路径导航
               isOneWay_=request.isOneWay; //( 正常路径导航 但是：起始点 在 结束点 的后面，小车没法倒着走，这个为false时就可以重新规划路径) 是否单向导航 true 单向导航，false 双向导航;当路径的起始点与结束点在一条路径上，且结束的点在起始点前方，是否允许它反转路径重新规划路径
              // isthresholdClosestPoint_=request.isthresholdClosestPoint; //（要在计算路径的时候给值）true:选择最近点作为切入和切出点（切入点与切出点相对固定，取决于起点和终点的位置【满足 切入点最小距离阀值，切出点最小距离阀值的范围内选择】）， false：选择组合路径距离最短的点作为切入和切出点（满足  切入点最小距离阀值，切出点最小距离阀值的范围内选择）

          //     if(state_machine_.get_current_state()=="idle"){
          //         contnav_info(__FILE__,__LINE__,"moveAction::goalCB ");
            //       ROS_INFO("moveAction::callback_global_plannerFun 开始导航");
             //      response.message=" moveAction::callback_global_plannerFun 开始导航！ ";
                   if(isSendGoalPlanOverturn_){//（直接反向规划）是否反转路径导航，true 反转路径导航，false 正常路径导航
                       int state_params_size=state_machine_.set_state("navigate_to_goalpathOverturn", {request.request_start,request.request_goal, request.request_default_goal,std::string("navigate_to_goalpathOverturn")});// target_pose_default_;//返回默认点位，或是充电点位
                       ROS_INFO("走 navigate_to_goalpathOverturn ");

                   }else{
                       int state_params_size=state_machine_.set_state("navigate_to_goalpath", {request.request_start,request.request_goal, request.request_default_goal,std::string("navigate_to_goalpath")});// target_pose_default_;//返回默认点位，或是充电点位
                       ROS_INFO("走 navigate_to_goalpath ");
                   }
//               }else{
//
//                   ROS_ERROR("moveAction::callback_global_plannerFun 导航失败，当前状态非空闲 idle,请等待任务结束 error");
//                   contnav_error(__FILE__,__LINE__,"moveAction::callback_global_plannerFun 导航失败，当前状态非空闲idle，  请等待任务结束 error ");
//                   response.message=" error moveAction::callback_global_plannerFun 导航失败，当前状态非空闲 idle,请等待任务结束 error！ ";
//               }


        }
        else if(request.request_type=="clear_costmaps") {
              //清空代价地图
            std::cout << " 清空代价地图 "  << std::endl;
            //恢复代价地图
            std_srvs::Empty srv;
            clear_costmaps_client_.call(srv);

        }
           else if(request.request_type=="cancelGoal") {
               init();
               ac_->cancelGoal();
           }//else if(request.request_type=="clear_costmaps")
           else if(request.request_type=="init") {
               init();

           }//else if(request.request_type=="clear_costmaps")
        //end-------------------------获取rviz登界面上取的点，或者从数据库取点 规划路径---------------------------------------
        std::cout << "8.路径规划 ================================================== " << std::endl;


        } catch (std::exception e) {
            //互锁
           // m_.unlock();
            cout << e.what() << endl;
            ROS_ERROR("callback_global_plannerFun()->exception ");
            response.message=" error 路径规划 callback_global_plannerFun()->exceptio！ ";

            response.result = ServiceResultType::error;

        }
        //延时
//        ros::Rate r((0.2));
//        r.sleep();
        //互锁
       // m_.unlock();
        if (fig) {
            response.result = ServiceResultType::ResultTypeOK;
        }
        //    std::cout << "====request.sqlStr==== "<< request.sqlStr << std::endl;

        return true;
    }









    void moveAction::run() {
        ros::NodeHandle private_nh("~");
        ros::NodeHandle private_nh2("~/" + global_planner_name_);
        //ros::NodeHandle action_nh("move_base_flex/move_base");//因为 move_base_flex中没有 move_base_simple/goal toptic，所以在这里需要制作一个与movebase中一致可用的 move_base_simple/goal
        ros::NodeHandle simple_nh("move_base_simple");//所以在这里需要制作一个与movebase 中一致可用的 move_base_simple/goal
        tf2_ros::TransformListener tfListener(buffer2);

        private_nh.param<string>("logFileName", logFileName, logFileName);//日志名称
        private_nh.param<string>("fileOutPut", fileOutPut_, fileOutPut_);//可配置路径
        private_nh.param<string>("move_base_global_toptic", move_base_global_toptic_, move_base_global_toptic_);//订阅全局地图   /move_base_flex/global_costmap/costmap  /move_base/global_costmap/costmap
        private_nh.param<string>("move_base_global_updates_toptic", move_base_global_updates_toptic_, move_base_global_updates_toptic_);// /move_base_flex/global_costmap/costmap_updates  /move_base/global_costmap/costmap_updates





        //private_nh.param<bool>("isthresholdClosestPoint", isthresholdClosestPoint_, isthresholdClosestPoint_);//true:选择最近点作为切入和切出点（切入点与切出点相对固定，取决于起点和终点的位置【满足 切入点最小距离阀值，切出点最小距离阀值的范围内选择】）， false：选择组合路径距离最短的点作为切入和切出点（满足  切入点最小距离阀值，切出点最小距离阀值的范围内选择）
        //private_nh.param<bool>("isSendGoalPlanOverturn", isSendGoalPlanOverturn_, isSendGoalPlanOverturn_);

        private_nh.param<bool>("isReturnPile", isReturnPile_, isReturnPile_);//超过时间： 是否返回充电桩 false绕障碍,rue回桩

        cout<<"1.moveAction::run() =======================moveAction::run() ===========================isReturnPile_:"<<(isReturnPile_?"true":"false")<<endl;

        //  private_nh.param<bool>("isSuspend", isSuspend_, isSuspend_);//true:暂停,false 绕障碍
        private_nh.param<string>("local_plan_toptic", local_plan_toptic_, local_plan_toptic_);//局部路径tooic名称

        private_nh.param<double>("wait_time", wait_time_, wait_time_);//遇到障碍物的等待超时时间



        // private_nh.param<bool>("isUseSemiStructured", isUseSemiStructured_, isUseSemiStructured_);//是否使用半结构化规划路径



        private_nh.param("base_global_planner", global_planner_name_, std::string("contnav_global_planner/global_plannerROS"));//navfn/NavfnROS

        private_nh.param("robot_base_frame", robot_base_frame_, std::string("base_link"));
        //            //是否探索未知区域，flase--不可到达
        private_nh.param("allow_unknown", allow_unknown_, false);

//            planner_->setHasUnknown(allow_unknown_);


        //窗口信息
        private_nh2.param("planner_window_x", planner_window_x_, 0.0);
        private_nh2.param("planner_window_y", planner_window_y_, 0.0);
        private_nh2.param("default_tolerance", default_tolerance_, 0.0);//#默认0.0，目标容差
        private_nh2.param("publish_scale", publish_scale_, 100);
        private_nh2.param("outline_map", outline_map_, true);

        /**
        这段代码是在创建一个名为"global_costmap"的costmap_2d::Costmap2DROS对象，
        其中tf_是一个tf::TransformListener对象。Costmap2DROS是一个ROS节点，
        它将costmap_2d::Costmap2D对象与ROS系统集成。它还提供了一些方便的功能，例如在ROS参数服务器上设置参数，以及在ROS中发布和订阅costmap。
        这个函数并没有直接订阅costmap数据。它创建了一个名为"global_costmap"的costmap_2d::Costmap2DROS对象，该对象将在ROS系统中发布和订阅costmap数据。在这个对象的构造函数中，它会调用costmap_2d::Costmap2DROS::start()函数，该函数会订阅ROS话题"/map"和"/map_updates"，并在ROS话题"/costmap_updates"上发布costmap数据。因此，这个函数创建的对象将订阅costmap数据。
        */
      //  controller_costmap_ros_ = new costmap_2d::Costmap2DROS("local_costmap", tf_);//局部规划地图
        //planner_costmap_ros_ = new costmap_2d::Costmap2DROS("global_costmap", tf_);//全局规划地图,//move_base的成本图, planner_costmap_ros_ = new costmap_2d::Costmap2DROS("global_costmap", tf_);//全局规划地图
        //planner_costmap_ros_->pause();//暂停全局代价地图的更新，但是不会关闭传感器话题的订阅
        //planner_costmap_ros_->start();//#订阅传感器话题，启动全局代价地图更新
       //planner_costmap_ros_->stop();//#停止全局代价地图更新，并停止订阅传感器话题
      //  planner_costmap_ros_->updateMap();//更新全局代价地图
      //  planner_costmap_ros_->resetLayers();//重置代价地图的所有图层
      //  planner_costmap_ros_->getCostmap()->setConvexPolygonCost(clear_poly, costmap_2d::FREE_SPACE);//设置多边形区域的代价值



        //todo [在move_base_flex abstract_navigation_server.cpp中增加的类似功能，这里可以去除] /home/sukai/workspace/workspace_ros_car_noetic/src/navigation-noetic/move_base_flex/move_base_flex/mbf_abstract_nav/src
        // 因为 move_base_flex中没有 move_base_simple/goal toptic，所以在这里需要制作一个与movebase中一致可用的 move_base_simple/goal
        //                                                        action_nh: move_base_flex/move_base/goal

        gc_ = std::make_shared<GotoCtrl>("move_base_flex/move_base", true);//点到点导航
        ac_ = std::make_shared<ExeCtrl>("/move_base_flex/exe_path", true);//ExeCtrl 给路径导航
        //todo 获取路径
        my_global_plannerClient = nh_.serviceClient<contnav_srvs::global_planner::Request, contnav_srvs::global_planner::Response>(
                "/my_global_planner");

        // 等待 Move_base_flex(mbf) 的服务
        while (!ac_->waitForServer(ros::Duration(5.0))||!gc_->waitForServer(ros::Duration(5.0)) )
        {
            ROS_WARN("等待 Move_base_flex(mbf) 的服务链接 ；Waiting for the move_base_flex action server to come up");
        }

        // 等待 Move_base_flex(mbf) 的服务
        while (!my_global_plannerClient.waitForExistence(ros::Duration(5.0)))
        {
            ROS_WARN("等待 my_global_plannerServer 半结构路径规划的服务链接 ；Waiting for the my_global_plannerServer server to come up");
        }

        //cmd_vel_client_暂停速度话题
        cmd_vel_client_ = nh_.serviceClient<std_srvs::SetBool>("/pause_cmd_vel");
       //                                                                     /move_base_simple/goal
        action_goal_pub_ = simple_nh.advertise<geometry_msgs::PoseStamped>("goal", 1);
        //todo           接收全局路径                                    simple_nh: /move_base_simple/traffic_goal
        goal_sub_ = simple_nh.subscribe<geometry_msgs::PoseStamped>("traffic_goal", 1, boost::bind(&moveAction::goalCB, this, _1));
       //发送全局路径
        goal_pub_ = simple_nh.advertise<geometry_msgs::PoseStamped>("traffic_goal", 1);
      //  traffic_goal_sub_ = nh.subscribe<geometry_msgs::PoseStamped>("/move_base_simple/traffic_goal", 1, &StateMachine::traffic_goal_cb, this);
        //---------------------------------------------------------------------------------------------------------------------------
        //feedbackCb中增加其它功能：1.检查机器人是否卡住。2.当机器人到达特定位置时执行其他操作（例如发布一条消息）。
        custom_pub_ = nh_.advertise<std_msgs::String>("/custom_topic", 1);


        //todo 订阅一个全局大地图，做障碍和珊格地图的提取  move_base_flex/global_costmap/costmap
                                                                       //  /move_base/global_costmap/costmap    /move_base_flex/global_costmap/costmap
        costmap_sub_ = private_nh.subscribe<nav_msgs::OccupancyGrid>(move_base_global_toptic_, 1,
                                                             &moveAction::grid_callback, this);
        costmap_updated_sub_ = private_nh.subscribe<map_msgs::OccupancyGridUpdate>(
                move_base_global_updates_toptic_, 1,&moveAction::grid_updated_callback, this);

        //todo 导航
      //  as_ = new MoveBaseActionServer(ros::NodeHandle(), "move_base", boost::bind(&MoveBase::executeCb, this, _1), false);

      //---------------------------------------------------------------------------------------------------------------

        // todo       用来判断实时障碍      订阅局部路径规划  /move_base_flex/TebLocalPlannerROS/local_plan
      //  local_path_sub_ = nh_.subscribe(local_plan_toptic_, 10, &moveAction::localPathCallback, this);

        //恢复代价地图
          clear_costmaps_client_ = nh_.serviceClient<std_srvs::Empty>("move_base_flex/clear_costmaps");

        //日志写入
        contnavLoggerClient = nh_.serviceClient<contnav_srvs::ContnavLogger::Request, contnav_srvs::ContnavLogger::Response>(
                "/contnavLogger");
        //数据库操作
        SqlOperationClient = nh_.serviceClient<contnav_srvs::TABLE_CONTORLLER::Request, contnav_srvs::TABLE_CONTORLLER::Response>(
                "/SqlOperation");





        subscriber_refreshdb = nh_.subscribe<std_msgs::String>("/refreshdb", 10, &moveAction::callBack_refreshdb, this);

        //接收控制指令
        global_plannerServer = nh_.advertiseService<contnav_srvs::moveAction::Request, contnav_srvs::moveAction::Response>(
                "/my_moveAction", boost::bind(&moveAction::callback_global_plannerFun, this, _1, _2));




          //发布速度
        cmd_vel_pub_ = nh_.advertise<geometry_msgs::Twist>("/cmd_vel", 10);

        //定时任务，这里会阻塞，因为这里需要查询数据库，需要提前启动数据库节点
       // timer2_ = nh_.createTimer(ros::Duration(1), &moveAction::updatefun, this, false, true);//数据表数据更新后2秒刷新数据
         //执行状态机
        boost::thread parse_thread(boost::bind(&moveAction::thread_msg, this));
         //发布速度停止指令
        boost::thread stopcmd_thread(boost::bind(&moveAction::stopCmd_thread, this));
//
//
//        dsrv_ = new dynamic_reconfigure::Server<contnav_global_planner::ContnavGlobalPlannerConfig>(ros::NodeHandle("~"));
//        dynamic_reconfigure::Server<contnav_global_planner::ContnavGlobalPlannerConfig>::CallbackType cb = boost::bind(&moveAction::reconfigureCB, this, _1, _2);
//        dsrv_->setCallback(cb);

        //private_nh.param<std::string>("map_frame", map_frame_, "map");
        //sub_ = node.subscribe<contnav_msgs::***>("/registerServiceResult", 10, &moveAction::msg_callBack, this);
        ros::spin();
    }



    //给充电桩发送充电指令
    void moveAction::chargeFun(const std::vector<boost::any> &args) {
        ROS_INFO("给充电桩发送充电指令");
        geometry_msgs::PoseStamped start_pose,target_pose,target_pose_default;
        state_machine_.set_state("idle", {start_pose,target_pose,target_pose_default,std::string("EMPTY")});//闲置状态,初始化给一个控制
    }


    void moveAction::doneCb(const actionlib::SimpleClientGoalState &state, const mbf_msgs::MoveBaseResultConstPtr &result) {
        ROS_INFO("点到点导航结果回调函数 doneCb Finished in state [%s]", state.toString().c_str());
        if (state == actionlib::SimpleClientGoalState::SUCCEEDED) {
            ROS_INFO("点到点   导航完成 Navigation succeeded.");
          //  state_machine_.set_state("idle", {});//闲置状态
        } else {
            ROS_ERROR("点到点 导航失败  Navigation failed. %s", state.getText().c_str());
            std::vector<boost::any> params;
          //  params.push_back(target_pose_);
           // params.push_back("");
           // state_machine_.set_state("navigate_to_goal", params);

        }
    }

    void moveAction::activeCb() {
        ROS_INFO("点到点 Goal is activeCb .");
       // state_machine_.set_state("idle", {});//闲置状态
    }
    // 在此处处理导航过程中的反馈
    void moveAction::feedbackCb(const mbf_msgs::MoveBaseFeedbackConstPtr &feedback) {
        ROS_INFO("点到点 导航过程中的反馈 feedbackCb Current position: x: %f, y: %f", feedback->current_pose.pose.position.x, feedback->current_pose.pose.position.y);
        //feedbackCb中增加其它功能：1.检查机器人是否卡住。2.当机器人到达特定位置时执行其他操作（例如发布一条消息）。
//        //// 检查导航是否卡住
//        double position_diff = sqrt(pow(feedback->current_pose.pose.position.x - last_pose_.pose.position.x, 2) +
//                                    pow(feedback->current_pose.pose.position.y - last_pose_.pose.position.y, 2));
//        // 如果差异很小，则可能意味着机器人卡住了
//        if (position_diff < position_diff_threshold_) {
//            if (!stuck_) {
//                ROS_WARN("点到点 Robot might be stuck!");
//                stuck_ = true;
//            }
//        } else {
//            stuck_ = false;
//        }
//        // 当机器人到达特定位置时执行其他操作（例如发布一条消息）
//        double target_distance = sqrt(pow(feedback->current_pose.pose.position.x - target_pose_.pose.position.x, 2) +
//                                      pow(feedback->current_pose.pose.position.y - target_pose_.pose.position.y, 2));
//        //distance_threshold_ 设置一个到达目标的距离阈值，当机器人到达目标位置时，将发布一条消息。
//        if (target_distance < distance_threshold_) {
//            std_msgs::String msg;
//            msg.data = "Robot reached the target area!";
//            custom_pub_.publish(msg);
//        }
//        // 更新上一次的位置
//        last_pose_ = feedback->current_pose;
    }

    //----

    void moveAction::activeCbExe() {
        ROS_INFO("路径导航 Goal is activeCbExe.");
    }


    // 导航结果回调函数
    void moveAction::resultCallbackExe(const actionlib::SimpleClientGoalState& state, const mbf_msgs::ExePathResultConstPtr& result)
    {

        try {

            cout<<"1.moveAction========================resultCallbackExe======================================================================"<<endl;

            cout<<"1.moveAction========================创建Twist消息并设置线速度和角速度======================================================================"<<endl;
            std::cout << "1.路径导航结果回调函数 resultCallbackExe Current state: " << state.toString() << std::endl;//PREEMPTED

            //获取状态机获取
            geometry_msgs::PoseStamped start_pose, target_pose, target_pose_default;
            std::string current_string;
            boost::any age0,age1,age2,age3;


            // 获取当前状态的成员函数
            std::string  current_state = state_machine_.get_current_state();
            std::cout << "2.current_state: " << current_state << std::endl;
            isSuspend_=true; //true:暂停,false 绕障碍
            if (state == actionlib::SimpleClientGoalState::SUCCEEDED)
            {
                restore();//恢复
                ROS_INFO("Navigation succeeded");
                // 获取当前状态的成员函数
                //std::string  current_state = state_machine_.get_current_state();
                std::cout << "2.current_state: " << current_state << std::endl;
                if(current_state !="idle"){
                    std::cout << "3.current_state: " << current_state << std::endl;

                    try
                    {
                        state_machine_.get_current_state_param(0,age0);//起始点
                        state_machine_.get_current_state_param(1,age1);//目标点
                        state_machine_.get_current_state_param(2,age2);//默认点
                        state_machine_.get_current_state_param(3,age3);//获取当前状态的参数

                        ROS_INFO_STREAM("2.创建Twist消息并设置线速度和角速度Type of args[0]: " << age0.type().name());
                        ROS_INFO_STREAM("2.创建Twist消息并设置线速度和角速度Type of args[1]: " << age1.type().name());
                        ROS_INFO_STREAM("2.创建Twist消息并设置线速度和角速度Type of args[2]: " << age2.type().name());
                        ROS_INFO_STREAM("2.创建Twist消息并设置线速度和角速度Type of args[3]: " << age3.type().name());

                        start_pose = any_cast<geometry_msgs::PoseStamped>(age0);
                        target_pose = any_cast<geometry_msgs::PoseStamped>(age1);
                        target_pose_default = any_cast<geometry_msgs::PoseStamped>(age2);
                        current_string = any_cast<std::string>(age3);
                        if(current_string=="charging" ) {
                            cout << "6.获取当前状态的参数current_string:" << current_string << endl;
                            int state_params_size=state_machine_.set_state("charging",{start_pose, target_pose, target_pose_default,current_string});///charging 充电
                            cout << "7.获取当前状态的参数current_string:" << current_string << endl;
                            cout<<"======int state_params_size======charging======state_params_size: "<<state_params_size<<endl;

                            return;
                        }

                    }
                    catch(...)
                    {
                        geometry_msgs::PoseStamped start_pose,target_pose,target_pose_default;
                        //int state_params_size=state_machine_.set_state("error", {start_pose,target_pose,target_pose_default,std::string("EMPTY")});//错误状态
                        state_machine_.set_state("idle", {start_pose,target_pose,target_pose_default,std::string("EMPTY")});//闲置状态,初始化给一个控制

                        ROS_ERROR("resultCallbackExe SUCCEEDED 获取数据 state_machine_.get_current_state_param 中的数据类型转换失败 ");
                        return;

                    }
                    std::cout << "3.1 current_state: " << current_state << std::endl;

                }//if(current_state !="idle")



              //  state_machine_.set_state("navigate_to_goalpathOverturn", {start_pose,target_pose, target_pose_default,std::string("charging")});///charging 充电


                //初始化给一个控制
                geometry_msgs::PoseStamped start_pose,target_pose,target_pose_default;
                state_machine_.set_state("idle", {start_pose,target_pose,target_pose_default,std::string("EMPTY")});//闲置状态,初始化给一个控制
                cout<<"2.moveAction========================创建Twist消息并设置线速度和角速度======================================================================"<<endl;

            }
          //  else if(state == actionlib::SimpleClientGoalState::PREEMPTED)
           else
            {

               if(state_machine_.get_current_state()!="monitor_progress"||state_machine_.get_current_state()=="idle" ){
                   cout<<"1.error 导航失败 result->outcome====="<<to_string(result->outcome)<<endl;

                   return;
               }


///**
                cout<<"3.error moveAction========================创建Twist消息并设置线速度和角速度=PREEMPTED====================================================================="<<endl;
                std::cout << "3.error 路径导航结果回调函数 resultCallbackExe Current state: " << state.toString() << std::endl;//PREEMPTED
                ROS_WARN("Navigation failed,发布Twist消息");

                // 创建Twist消息并设置线速度和角速度
                geometry_msgs::Twist twist_msg;
                cout<<"4.moveAction========================创建Twist消息并设置线速度和角速度=================================================================result->outcome====="<<to_string(result->outcome)<<endl;
                // 发布Twist消息
                geometry_msgs::Twist cmd_vel;
                cmd_vel.linear.x = 0;
                cmd_vel.linear.y = 0;
                cmd_vel.linear.z = 0;
                cmd_vel.angular.x = 0;
                cmd_vel.angular.y = 0;
                cmd_vel.angular.z = 0;
                cmd_vel_pub_.publish(cmd_vel);
                //result->outcome==101|| move_base_flex（MBF）包，那么101可能表示"FAILURE"，这是一个通用的错误代码，表示路径执行失败。
                if(result->outcome==110||result->outcome==101|| result->outcome==105||result->outcome==102||result->outcome==188){//105震荡超时 重新发布目标点 ，102计算速度命令失败，机器人无法继续导航，188是我自己定义的主要是遇到障碍物/home/sukai/workspace/workspace_ros_car_noetic/src/navigation-noetic/move_base_flex/move_base_flex/mbf_msgs/action/ExePath.action  uint8 NO_VALID_CMD_FEASIBLE  = 188 #sukai， teb 规划 if (!feasible)
                    ROS_INFO("Navigation failed,发布Twist消息");
                    //std::string  current_state = state_machine_.get_current_state();
                    std::cout << "00.current_state 超时或者没路径current_state: " << current_state << std::endl;
                    if(current_state=="error" ||current_state=="idle" )
                        return;



                    try
                    {
                        state_machine_.get_current_state_param(0,age0);//起始点
                        state_machine_.get_current_state_param(1,age1);//目标点
                        state_machine_.get_current_state_param(2,age2);//默认点
                        state_machine_.get_current_state_param(3,age3);//获取当前状态的参数

                        ROS_INFO_STREAM("3.创建Twist消息并设置线速度和角速度Type of args[0]: " << age0.type().name());
                        ROS_INFO_STREAM("3.创建Twist消息并设置线速度和角速度Type of args[1]: " << age1.type().name());
                        ROS_INFO_STREAM("3.创建Twist消息并设置线速度和角速度Type of args[2]: " << age2.type().name());
                        ROS_INFO_STREAM("3.创建Twist消息并设置线速度和角速度Type of args[3]: " << age3.type().name());


                        start_pose = any_cast<geometry_msgs::PoseStamped>(age0);
                        target_pose = any_cast<geometry_msgs::PoseStamped>(age1);
                        target_pose_default = any_cast<geometry_msgs::PoseStamped>(age2);
                        current_string = any_cast<std::string>(age3);
                    }
                    catch(...)
                    {

                        ROS_ERROR("11111resultCallbackExe 获取数据 state_machine_.get_current_state_param 中的数据类型转换失败 ");
                        //geometry_msgs::PoseStamped start_pose,target_pose,target_pose_default;
                        state_machine_.set_state("error", {start_pose,target_pose,target_pose_default,std::string("EMPTY")});//错误状态
                        return;

                    }

                    std::cout << "4.current_string 超时或者没路径: " << current_string << std::endl;
                    if(current_string =="charging"){//翻转导航，回充电桩  //todo 这里会震荡，需要优化 ,已经改了这两个值charging1,"1"，后面需要改movebase的参数 oscillation_timeout 把震荡超时间调大,要比代码中的wait_time_超时时间大，在把teb中的 到达目标点的数据调大，精度调低
                        state_machine_.set_state("navigate_to_goalpathDefaultgoal", {start_pose,target_pose, target_pose_default,current_string});///charging 充电
                        restore2();
                    }else if(current_string =="navigate_to_goalpath"|| current_string =="navigate_to_goalpath_goalCB") {////返回默认点位，或是充电点位
                        state_machine_.set_state("navigate_to_goalpath", {start_pose,target_pose, target_pose_default,current_string});// t正常导航
                        restore2();
                    }else if(current_string =="navigate_to_goalpathOverturn") {
                        state_machine_.set_state("navigate_to_goalpathOverturn", {start_pose,target_pose, target_pose_default,current_string});// target_pose_default_;
                        restore2();
                    }



                   // if(!target_pose_.header.frame_id.empty()){
                       // geometry_msgs::PoseStamped  goalPose;
                      //  goalPose=target_pose_;
                       // goalPose.header.stamp = ros::Time::now();

                        try {
                            //恢复代价地图
                            std_srvs::Empty srv;
                            clear_costmaps_client_.call(srv);
                            //if(!target_pose_.header.frame_id.empty()){
                               // ac_->cancelGoal();
                               // goal_pub_.publish(target_pose_);
                               // goal_pub_.publish(target_pose);
                                ROS_INFO("Navigation failed,重新发布目标点");
                            //}

                        } catch (const std::exception& e) {
                            ROS_ERROR("Exception 第二次半结构化路径规划失败 caught while calling sendGoalPlan(): %s", e.what());
                        } catch (...) {
                            ROS_ERROR("Unknown 第二次半结构化路径规划失败 exception caught while calling sendGoalPlan()");
                        }
                        //   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

//                    }else{
//                        ROS_DEBUG_NAMED("goalCB ","第二次半结构化路径规划失败");
//                    }


                }
//                else if(result->outcome==110){
//
//
//                    cout<<"5.moveAction========================创建Twist消息并设置线速度和角速度============ABORTED===目标点位被障碍物占用=============result->outcome====="<<to_string(result->outcome)<<endl;
//                    //取消上一次任务
//                    ac_->cancelGoal();
//                    if(current_string =="navigate_to_goalpath_goalCB") {////返回默认点位，或是充电点位
//                         state_machine_.set_state("idle", {start_pose,target_pose,target_pose_default,std::string("EMPTY")});//闲置状态,初始化给一个控制
//                    }else{
//                        state_machine_.set_state("navigate_to_goalpathDefaultgoal", {start_pose,target_pose, target_pose_default,current_string});///charging 充电
//                    }
//                    restore();//恢复
//                }
//*/

                cout<<"6.moveAction========================创建Twist消息并设置线速度和角速度=================================================================result->outcome====="<<to_string(result->outcome)<<endl;

                ///   mbf_msgs::ExePathResult::NO_VALID_CMD;
            }
//            else if(state == actionlib::SimpleClientGoalState::ABORTED){//错误
//                cout<<"5.moveAction========================创建Twist消息并设置线速度和角速度============ABORTED================result->outcome====="<<to_string(result->outcome)<<endl;
//                state_machine_.set_state("idle", {start_pose,target_pose,target_pose_default,std::string("EMPTY")});//闲置状态,初始化给一个控制
//                //取消上一次任务
//                ac_->cancelGoal();
//                restore();//恢复
//            }
//            cout<<"5.========================创建Twist消息并设置线速度和角速度======================================================================"<<endl;
//            //恢复代价地图
//            std_srvs::Empty srv;
//            clear_costmaps_client_.call(srv);
//            //帮我打印 state
//            std::cout << "Current state: " << state.toString() << std::endl;
//            //cout<<"state: "<< state<<endl;
//            ROS_INFO("导航结果回调函数 Navigation result received result->outcome : %d", result->outcome);
//            ROS_INFO("导航结果回调函数 Navigation result received:SUCCEEDED %d", actionlib::SimpleClientGoalState::SUCCEEDED);
//            ROS_INFO("导航结果回调函数 Navigation result received:ABORTED %d", actionlib::SimpleClientGoalState::ABORTED);
//            ROS_INFO("导航结果回调函数 Navigation result received:ACTIVE %d", actionlib::SimpleClientGoalState::ACTIVE);
//            ROS_INFO("导航结果回调函数 Navigation result received:LOST %d", actionlib::SimpleClientGoalState::LOST);
//            ROS_INFO("导航结果回调函数 Navigation result received:PENDING %d", actionlib::SimpleClientGoalState::PENDING);
//            ROS_INFO("导航结果回调函数 Navigation result received:PREEMPTED %d", actionlib::SimpleClientGoalState::PREEMPTED);
//            ROS_INFO("导航结果回调函数 Navigation result received:RECALLED %d", actionlib::SimpleClientGoalState::RECALLED);
//            ROS_INFO("导航结果回调函数 Navigation result received:REJECTED %d", actionlib::SimpleClientGoalState::REJECTED);




        } catch (const std::exception& e) {
            ROS_ERROR("Exception resultCallbackcaught while calling sendGoalPlan() in resultCallback(): %s", e.what());
        } catch (...) {
            ROS_ERROR("Unknown resultCallbackexception caught while calling sendGoalPlan() in resultCallback()");
        }



       // restore();//恢复

        // 结束 ROS 循环
        // ros::shutdown();
    }
    // 在此处处理导航过程中的反馈
    void moveAction::feedbackCbExe(const mbf_msgs::ExePathFeedbackConstPtr &feedback) {
        //feedbackCb中增加其它功能：1.检查机器人是否卡住。2.当机器人到达特定位置时执行其他操作（例如发布一条消息）。
        //// 检查导航是否卡住
        double position_diff = sqrt(pow(feedback->current_pose.pose.position.x - last_pose_.pose.position.x, 2) +
                                    pow(feedback->current_pose.pose.position.y - last_pose_.pose.position.y, 2));
        // 如果差异很小，则可能意味着机器人卡住了
        if (position_diff < position_diff_threshold_) {
            if (!stuck_) {
                ROS_WARN("机器人可能被卡住 Robot might be stuck!");
                stuck_ = true;
            }
        } else {
            stuck_ = false;
        }
      //获取状态机获取
        geometry_msgs::PoseStamped start_pose, target_pose, target_pose_default;
        std::string extra_string;
        boost::any age0,age1,age2,age3;
        try
        {
            state_machine_.get_current_state_param(0,age0);//起始点
            state_machine_.get_current_state_param(1,age1);//目标点
            state_machine_.get_current_state_param(2,age2);//默认点
            state_machine_.get_current_state_param(3,age3);//获取当前状态的参数

            start_pose = any_cast<geometry_msgs::PoseStamped>(age0);
            target_pose = any_cast<geometry_msgs::PoseStamped>(age1);
            target_pose_default = any_cast<geometry_msgs::PoseStamped>(age2);
            extra_string = any_cast<std::string>(age3);
        }
        catch(...)
        {
            // handle the exception
            ROS_ERROR("1. navigate_to_goalpath 获取状态机数据 失败extra_string %s", extra_string.c_str() );
            return ;
        }

        // 当机器人到达特定位置时执行其他操作（例如发布一条消息）
        double target_distance = sqrt(pow(feedback->current_pose.pose.position.x - target_pose.pose.position.x, 2) +
                                      pow(feedback->current_pose.pose.position.y - target_pose.pose.position.y, 2));
        //distance_threshold_ 设置一个到达目标的距离阈值，当机器人到达目标位置时，将发布一条消息。
        if (target_distance < distance_threshold_) {
            std_msgs::String msg;
            msg.data = "Robot reached the target area Robot reached the target area!";
            custom_pub_.publish(msg);
        }
        // 更新上一次的位置
        last_pose_ = feedback->current_pose;
    }



    //半结构化路径规划traffic_goal
    void moveAction::goalCB(const geometry_msgs::PoseStamped::ConstPtr& goal){
        ROS_DEBUG_NAMED("1.半结构化路径规划traffic_goal -> goalCB ","半结构化路径规划");
        restore();//恢复

        //查询是否存在半结构化路径规划
        geometry_msgs::PoseStamped target_pose = *goal;//目标点位置
        contnav_srvs::global_planner global_planner_srv;
        global_planner_srv.request.request_type = "getExecuteMapPlanSize";
        //通过service获取路径
        my_global_plannerClient.call(global_planner_srv);
        ROS_DEBUG_NAMED("2.goalCB my_global_plannerClient.call(global_planner_srv): ","查询是否存在半结构化路径规划");
        if(global_planner_srv.response.result!=ServiceResultType::ResultTypeOK){
            // action_goal_pub_.publish(*goal);
            mbf_msgs::MoveBaseGoal goal;
            goal.target_pose = target_pose;
            gc_->sendGoal(goal,
                          boost::bind(&moveAction::doneCb, this, _1, _2),
                          boost::bind(&moveAction::activeCb, this),
                          boost::bind(&moveAction::feedbackCb, this, _1));

            cout<<"半结构化路径，就用正常的 goal 发出去"<<endl;
            cout<<"moveAction::goalCB 获取路径失败"<<endl;
            return;
        }
        ROS_DEBUG_NAMED("3. my_global_plannerClient.call(global_planner_srv): ","查询是否存在半结构化路径规划");


        geometry_msgs::PoseStamped start_pose;
        if(!getRobotPose(start_pose)){
            ROS_ERROR("goalCB my_moveAction cannot make a plan for you because it could not get the start pose of the robot");
            //日志
            //  contnav_error(__FILE__,__LINE__,const_cast<char*>(response.message.c_str()));
            //  contnav_error(__FILE__,__LINE__,"moveAction::goalCB 获取小车当前位置 error");
            return ;
        }
        ROS_DEBUG_NAMED("3. goalCB if(!getRobotPose(start_pose)): ","start_pose");


     //   if(state_machine_.get_current_state()=="idle"){
       //     contnav_info(__FILE__,__LINE__,"moveAction::goalCB ");
       //     ROS_INFO("moveAction::goalCB 开始导航");
             geometry_msgs::PoseStamped target_pose_default;
             ROS_DEBUG_NAMED("4. goalCB if(!getRobotPose(start_pose)): ","target_pose_default");
             int state_params_size=state_machine_.set_state("navigate_to_goalpath", {start_pose,target_pose, target_pose_default,std::string("navigate_to_goalpath_goalCB")});// target_pose_default_;//返回默认点位，或是充电点位
             ROS_DEBUG_NAMED("5. goalCB if(!getRobotPose(start_pose)): ","navigate_to_goalpath");

             //        }
//        else{
//            string msgebendi="moveAction::goalCB 导航失败，当前状态非空闲 idle,请等待任务结束 error"+state_machine_.get_current_state();
//              ROS_ERROR(msgebendi.c_str());
//              contnav_error(__FILE__,__LINE__,"moveAction::goalCB 导航失败，当前状态非空闲idle，  请等待任务结束 error ");
//        }


    }


    // 调用 MBF 客户端导航至目标点
    void moveAction::navigate_to_goalpath(const std::vector<boost::any> &args) {
        ROS_INFO("Navigating to goal...");
        ROS_INFO_STREAM("Type of args[1]: " << args[1].type().name());
        ROS_INFO_STREAM("Type of args[3]: " << args[3].type().name());



        geometry_msgs::PoseStamped start_pose, target_pose, target_pose_default;
        std::string extra_string="EMPTY";
        try
        {
            start_pose = any_cast<geometry_msgs::PoseStamped>(args[0]);
            target_pose = any_cast<geometry_msgs::PoseStamped>(args[1]);
            target_pose_default = any_cast<geometry_msgs::PoseStamped>(args[2]);
            extra_string = any_cast<std::string>(args[3]);
        }
        catch(...)
        {
            // handle the exception
            ROS_ERROR("1. navigate_to_goalpath 获取状态机数据 失败extra_string %s", extra_string.c_str() );
            return ;
        }

        ROS_INFO_STREAM("Type of extra_string: " << extra_string);
        // geometry_msgs::PoseStamped target_pose = boost::any_cast<geometry_msgs::PoseStamped>(args);
        //获取小车当前位置
        geometry_msgs::PoseStamped robot_pose;
        if(!getRobotPose(robot_pose)){
            ROS_ERROR("my_global_planner cannot make a plan for you because it could not get the start pose of the robot");
            //日志
            //  contnav_error(__FILE__,__LINE__,const_cast<char*>(response.message.c_str()));
            //  contnav_error(__FILE__,__LINE__,"global_planner::goalCB 获取小车当前位置 error");
            return ;
        }



        contnav_srvs::global_planner global_planner_srv;
        global_planner_srv.request.request_type = global_planner_srv.request.waypoint;
        global_planner_srv.request.waypointAction = global_planner_srv.request.GETPLAN;
        global_planner_srv.request.isSendGoalPlanOverturn = false;//  是否反转路径导航，true 反转路径导航，false 正常路径导航
        global_planner_srv.request.isOneWay = isOneWay_;  //( 起始点 在 结束点 的后面，小车没法倒着走，这个为false时就可以重新规划路径) 是否单向导航 true 单向导航，false 双向导航;当路径的起始点与结束点在一条路径上，且结束的点在起始点前方，是否允许它反转路径重新规划路径
       // global_planner_srv.request.isthresholdClosestPoint = isthresholdClosestPoint_;// true:选择最近点作为切入和切出点（切入点与切出点相对固定，取决于起点和终点的位置【满足 切入点最小距离阀值，切出点最小距离阀值的范围内选择】）， false：选择组合路径距离最短的点作为切入和切出点（满足  切入点最小距离阀值，切出点最小距离阀值的范围内选择）
        global_planner_srv.request.request_goal = target_pose;//  是否发送路径导航，true 发送路径导航，false 不发送路径导航
        //通过service获取路径
        my_global_plannerClient.call(global_planner_srv);
        if(global_planner_srv.response.result==ServiceResultType::ResultTypeOK){
            //  contnav_info(__FILE__,__LINE__,"moveAction::goalCB 获取路径成功");
            cout<<"moveAction::goalCB 获取路径成功"<<endl;

            mbf_msgs::ExePathGoal result_goal_path;
            for (int i = 0; i < global_planner_srv.response.result_plan.size(); ++i) {
                global_planner_srv.response.result_plan[i].header.frame_id=frame_id_;
                global_planner_srv.response.result_plan[i].header.stamp=ros::Time::now();
                result_goal_path.path.poses.push_back(global_planner_srv.response.result_plan[i]);
            }
            ROS_DEBUG_NAMED("navigate_to_goalpath ","半结构化路径规划成功,开始导航。");
            ac_->sendGoal(result_goal_path, boost::bind(&moveAction::resultCallbackExe, this, _1, _2),
                          boost::bind(&moveAction::activeCbExe, this),
                          boost::bind(&moveAction::feedbackCbExe, this, _1));

            //设置next_state下一个状态和参数  navigate_to_goalpath
            int state_params_size=state_machine_.set_state("monitor_progress", {start_pose,target_pose,target_pose_default, extra_string});
            cout<<"w======int state_params_size====monitor_progress========state_params_size: "<<state_params_size<<" ,extra_string: "<<extra_string<<endl;
        }
        else{
            //  contnav_error(__FILE__,__LINE__,"moveAction::goalCB 获取路径失败");
            cout<<"moveAction::navigate_to_goalpath 获取路径失败"<<endl;
            //设置next_state下一个状态和参数  navigate_to_goalpath
            //int state_params_size=state_machine_.set_state("monitor_progress", {start_pose,target_pose,target_pose_default, extra_string});
            //cout<<"w======int state_params_size====monitor_progress========state_params_size: "<<state_params_size<<" ,extra_string: "<<extra_string<<endl;
            //初始化给一个控制
            state_machine_.set_state("idle", {start_pose,target_pose,target_pose_default,std::string("EMPTY")});//闲置状态,初始化给一个控制
        }
    }



    // 调用 MBF 客户端导航至目标点 (翻转路径)
    void moveAction::navigate_to_goalpathOverturn(const std::vector<boost::any> &args) {

        ROS_INFO("Navigating to goal...");
        ROS_INFO_STREAM("Type of args[1]: " << args[1].type().name());
//        geometry_msgs::PoseStamped start_pose = boost::any_cast<geometry_msgs::PoseStamped>(args[0]);
//        geometry_msgs::PoseStamped target_pose = boost::any_cast<geometry_msgs::PoseStamped>(args[1]);
//        geometry_msgs::PoseStamped target_pose_default = boost::any_cast<geometry_msgs::PoseStamped>(args[2]);//默认点位，可以从数据库中读取，可以回充电装
//        std::string extra_string = boost::any_cast<std::string>(args[3]);

        geometry_msgs::PoseStamped start_pose, target_pose, target_pose_default;
        std::string extra_string="EMPTY";
        try
        {
            start_pose = any_cast<geometry_msgs::PoseStamped>(args[0]);
            target_pose = any_cast<geometry_msgs::PoseStamped>(args[1]);
            target_pose_default = any_cast<geometry_msgs::PoseStamped>(args[2]);
            extra_string = any_cast<std::string>(args[3]);
        }
        catch(...)
        {
            // handle the exception
            ROS_INFO("1. navigate_to_goalpathOverturn 获取状态机数据 失败extra_string %s", extra_string.c_str() );
            return ;
        }


        //

        // geometry_msgs::PoseStamped target_pose = boost::any_cast<geometry_msgs::PoseStamped>(args);
        //获取小车当前位置
        geometry_msgs::PoseStamped robot_pose;
        if(!getRobotPose(robot_pose)){
            ROS_ERROR("my_global_planner cannot make a plan for you because it could not get the start pose of the robot");
            //日志
            //  contnav_error(__FILE__,__LINE__,const_cast<char*>(response.message.c_str()));
            //  contnav_error(__FILE__,__LINE__,"global_planner::goalCB 获取小车当前位置 error");
            return ;
        }


        contnav_srvs::global_planner global_planner_srv;
        global_planner_srv.request.request_type = global_planner_srv.request.waypoint;
        global_planner_srv.request.waypointAction = global_planner_srv.request.GETPLAN;
        global_planner_srv.request.isSendGoalPlanOverturn = true;//  是否反转路径导航，true 反转路径导航，false 正常路径导航
        global_planner_srv.request.isOneWay = isOneWay_;  //( 起始点 在 结束点 的后面，小车没法倒着走，这个为false时就可以重新规划路径) 是否单向导航 true 单向导航，false 双向导航;当路径的起始点与结束点在一条路径上，且结束的点在起始点前方，是否允许它反转路径重新规划路径
        //global_planner_srv.request.isthresholdClosestPoint = isthresholdClosestPoint_;// true:选择最近点作为切入和切出点（切入点与切出点相对固定，取决于起点和终点的位置【满足 切入点最小距离阀值，切出点最小距离阀值的范围内选择】）， false：选择组合路径距离最短的点作为切入和切出点（满足  切入点最小距离阀值，切出点最小距离阀值的范围内选择）
        global_planner_srv.request.request_goal = target_pose;//  是否发送路径导航，true 发送路径导航，false 不发送路径导航
        //通过service获取路径
        my_global_plannerClient.call(global_planner_srv);
        if(global_planner_srv.response.result==ServiceResultType::ResultTypeOK){
            //  contnav_info(__FILE__,__LINE__,"moveAction::goalCB 获取路径成功");
            cout<<"moveAction::goalCB 获取路径成功"<<endl;

            mbf_msgs::ExePathGoal result_goal_path;
            for (int i = 0; i < global_planner_srv.response.result_plan.size(); ++i) {
                global_planner_srv.response.result_plan[i].header.frame_id=frame_id_;
                global_planner_srv.response.result_plan[i].header.stamp=ros::Time::now();
                result_goal_path.path.poses.push_back(global_planner_srv.response.result_plan[i]);
            }
            ROS_DEBUG_NAMED("goalCB ","半结构化路径规划成功");
            ac_->sendGoal(result_goal_path, boost::bind(&moveAction::resultCallbackExe, this, _1, _2),
                          boost::bind(&moveAction::activeCbExe, this),
                          boost::bind(&moveAction::feedbackCbExe, this, _1));

            //设置next_state下一个状态和参数  navigate_to_goalpath
            int state_params_size=state_machine_.set_state("monitor_progress", {start_pose,target_pose,target_pose_default, extra_string});
        }else{
//            ROS_DEBUG_NAMED("goalCB ","半结构化路径规划失败");
//            std::cout << " 清空代价地图 "  << std::endl;
//            //恢复代价地图
//            std_srvs::Empty srv;
//            clear_costmaps_client_.call(srv);
//
//            if(state_machine_.get_current_state() == "navigate_to_goalpathOverturn"){
//                //重新发布一次
//                int state_params_size=state_machine_.set_state("navigate_to_goalpathOverturnAgain", {start_pose,target_pose, target_pose_default,extra_string});
//                ROS_INFO("navigate_to_goalpath  再次发送导航: %s", state_machine_.get_current_state().c_str() );
//                cout<<"e=====int state_params_size====navigate_to_goalPathAgain========state_params_size: "<<state_params_size<<endl;
//            }else{
//                ROS_ERROR("navigate_to_goalpath 不能再次发送导航失败: %s", state_machine_.get_current_state().c_str() );
//                //  state_machine_.set_state("navigate_to_goalpathAgain", {start_pose,target_pose,target_pose_default, extra_string});
//              //  int state_params_size=state_machine_.set_state("error", {start_pose,target_pose, target_pose_default,std::string("error")});
//                //  cout<<"e=====int state_params_size====error========state_params_size: "<<state_params_size<<endl;
//                state_machine_.set_state("idle", {start_pose,target_pose,target_pose_default,std::string("EMPTY")});//闲置状态,初始化给一个控制
//
//            }
            cout<<"moveAction::navigate_to_goalpathOverturn 获取路径失败"<<endl;
            //初始化给一个控制
            state_machine_.set_state("idle", {start_pose,target_pose,target_pose_default,std::string("EMPTY")});//闲置状态,初始化给一个控制
            return;
        }



    }//==


    // 调用 MBF 客户端导航至目标点 (翻转路径)
    void moveAction::navigate_to_goalpathDefaultgoal(const std::vector<boost::any> &args) {

        ROS_INFO("Navigating to navigate_to_goalpathOverturn ...");
        ROS_INFO_STREAM("Type of args[1]: " << args[1].type().name());
//        geometry_msgs::PoseStamped start_pose = boost::any_cast<geometry_msgs::PoseStamped>(args[0]);
//        geometry_msgs::PoseStamped target_pose = boost::any_cast<geometry_msgs::PoseStamped>(args[1]);
//        geometry_msgs::PoseStamped target_pose_default = boost::any_cast<geometry_msgs::PoseStamped>(args[2]);//默认点位，可以从数据库中读取，可以回充电装
//        std::string extra_string = boost::any_cast<std::string>(args[3]);


        //获取数据
        geometry_msgs::PoseStamped start_pose, target_pose, target_pose_default;
        std::string extra_string="EMPTY";
        try
        {
            boost::any age0=args[0];
            boost::any age1=args[1];
            boost::any age2=args[2];
            boost::any age3=args[3];
            state_machine_.get_current_state_param(0,age0);
            state_machine_.get_current_state_param(1,age1);
            state_machine_.get_current_state_param(2,age2);
            state_machine_.get_current_state_param(3,age3);
            start_pose = any_cast<geometry_msgs::PoseStamped>(age0);
            target_pose = any_cast<geometry_msgs::PoseStamped>(age1);
            target_pose_default = any_cast<geometry_msgs::PoseStamped>(age2);
            extra_string = any_cast<std::string>(age3);
        }
        catch(...)
        {
            // handle the exception
            ROS_ERROR("1. navigate_to_goalpath 获取状态机数据 失败extra_string %s", extra_string.c_str() );
            return ;
        }

        //切换正在执行状态
//        int state_params_size=state_machine_.set_state("executing", {start_pose,target_pose,target_pose_default, std::string("navigate_to_goalpathDefaultgoal")});
//
//        if(state_machine_.get_current_state()=="navigate_to_goalpathDefaultgoal" && extra_string=="EMPTY"){
//            //切换正在执行状态
//            int state_params_size=state_machine_.set_state("executing", {start_pose,target_pose,target_pose_default, std::string("charging")});
//
//        }else if(state_machine_.get_current_state()=="navigate_to_goalpathDefaultgoal" && extra_string=="navigate_to_goalpathDefaultgoalAain") {
//            //切换正在执行状态
//            int state_params_size=state_machine_.set_state("executing", {start_pose,target_pose,target_pose_default, std::string("charging")});
//
//        }

        // geometry_msgs::PoseStamped target_pose = boost::any_cast<geometry_msgs::PoseStamped>(args);

        //获取小车当前位置
        geometry_msgs::PoseStamped robot_pose;
        if(!getRobotPose(robot_pose)){
            ROS_ERROR("my_global_planner cannot make a plan for you because it could not get the start pose of the robot");
            //日志
            //  contnav_error(__FILE__,__LINE__,const_cast<char*>(response.message.c_str()));
            //  contnav_error(__FILE__,__LINE__,"global_planner::goalCB 获取小车当前位置 error");
            return ;
        }
        contnav_srvs::global_planner global_planner_srv;
        global_planner_srv.request.request_type = global_planner_srv.request.waypoint;
        global_planner_srv.request.waypointAction = global_planner_srv.request.GETPLAN;
        global_planner_srv.request.isSendGoalPlanOverturn =isSendGoalPlanOverturn_?false:true;//  是否反转路径导航，true 反转路径导航，false 正常路径导航
        global_planner_srv.request.isOneWay = isOneWay_;  //( 起始点 在 结束点 的后面，小车没法倒着走，这个为false时就可以重新规划路径) 是否单向导航 true 单向导航，false 双向导航;当路径的起始点与结束点在一条路径上，且结束的点在起始点前方，是否允许它反转路径重新规划路径
        //global_planner_srv.request.isthresholdClosestPoint = isthresholdClosestPoint_;// true:选择最近点作为切入和切出点（切入点与切出点相对固定，取决于起点和终点的位置【满足 切入点最小距离阀值，切出点最小距离阀值的范围内选择】）， false：选择组合路径距离最短的点作为切入和切出点（满足  切入点最小距离阀值，切出点最小距离阀值的范围内选择）
        global_planner_srv.request.request_goal = target_pose_default;//  是否发送路径导航，true 发送路径导航，false 不发送路径导航
        //通过service获取路径
        my_global_plannerClient.call(global_planner_srv);
        if(global_planner_srv.response.result==ServiceResultType::ResultTypeOK){
            //  contnav_info(__FILE__,__LINE__,"moveAction::goalCB 获取路径成功");
            cout<<"moveAction::goalCB 获取路径成功"<<endl;

            mbf_msgs::ExePathGoal result_goal_path;
            for (int i = 0; i < global_planner_srv.response.result_plan.size(); ++i) {
                global_planner_srv.response.result_plan[i].header.frame_id=frame_id_;
                global_planner_srv.response.result_plan[i].header.stamp=ros::Time::now();
                result_goal_path.path.poses.push_back(global_planner_srv.response.result_plan[i]);
            }
            ROS_DEBUG_NAMED("goalCB ","半结构化路径规划成功");
            ac_->sendGoal(result_goal_path, boost::bind(&moveAction::resultCallbackExe, this, _1, _2),
                          boost::bind(&moveAction::activeCbExe, this),
                          boost::bind(&moveAction::feedbackCbExe, this, _1));

            //设置next_state下一个状态和参数  navigate_to_goalpath
            int state_params_size=state_machine_.set_state("monitor_progress", {start_pose,target_pose,target_pose_default, std::string("charging")});
            cout<<"w======int state_params_size====monitor_progress========state_params_size: "<<state_params_size<<" ,extra_string: "<<extra_string<<endl;
        }else{
//            ROS_DEBUG_NAMED("goalCB ","半结构化路径规划失败");
//            std::cout << " 清空代价地图 "  << std::endl;
//            //恢复代价地图
//            std_srvs::Empty srv;
//            clear_costmaps_client_.call(srv);
//
//            if(state_machine_.get_current_state() == "navigate_to_goalpathDefaultgoal"){
//                //重新发布一次
//                int state_params_size=state_machine_.set_state("navigate_to_goalpathDefaultgoalAgain", {start_pose,target_pose, target_pose_default,extra_string});
//                ROS_INFO("navigate_to_goalpathDefaultgoal  再次发送导航: %s", state_machine_.get_current_state().c_str() );
//                cout<<"e=====int state_params_size====navigate_to_goalpathDefaultgoalAgain========state_params_size: "<<state_params_size<<endl;
//            }else{
//                ROS_ERROR("navigate_to_goalpathDefaultgoal 不能再次发送导航失败: %s", state_machine_.get_current_state().c_str() );
//                //  state_machine_.set_state("navigate_to_goalpathDefaultgoalAgain", {start_pose,target_pose,target_pose_default, extra_string});
//              //  int state_params_size=state_machine_.set_state("error", {start_pose,target_pose, target_pose_default,std::string("error")});
//                //  cout<<"e=====int state_params_size====error========state_params_size: "<<state_params_size<<endl;
//                state_machine_.set_state("idle", {start_pose,target_pose,target_pose_default,std::string("EMPTY")});//闲置状态,初始化给一个控制
//
//            }
//            cout<<"moveAction::goalCB 获取路径失败"<<endl;

            cout<<"moveAction::navigate_to_goalpathDefaultgoal 获取路径失败"<<endl;
            //初始化给一个控制
            state_machine_.set_state("idle", {start_pose,target_pose,target_pose_default,std::string("EMPTY")});//闲置状态,初始化给一个控制
            return;
        }

    }//========================

    // 切换到监控状态,监控是否遇到障碍物，遇到就停止
    void moveAction::monitor_progress(const std::vector<boost::any> &args) {
        // ROS_INFO("1.切换到监控进度状态 切换到监控状态,监控是否遇到障碍物，遇到就停止 Monitoring progress.state_machine_.get_current_state(): %s", state_machine_.get_current_state().c_str() );


        std::string extra_string="EMPTY";
        std_srvs::SetBool srv;
        try
        {
            extra_string = any_cast<std::string>(args[3]);
            // ROS_INFO("1.====monitor_progress========================================== extra_string: %s", extra_string.c_str() );

        }
        catch(...)
        {
            // handle the exception
            ROS_ERROR("2.获取状态机数据monitor_progress 失败extra_string %s", extra_string.c_str() );
            return;
        }
        //  ROS_INFO("2.切换到监控进度状态 切换到监控状态,监控是否遇到障碍物，遇到就停止 extra_string: %s", extra_string.c_str() );

       // if(extra_string == "charging" ||!isReturnPile_){// true:暂停,false 绕障碍物
            if(extra_string == "charging" || !isSuspend_){ // isSuspend_= false; //true:暂停,false 绕障碍
            obstacle_detected_time_ = ros::Time::now();
            stopCmdFig_= false;//发布速度停止指令

            srv.request.data = false;  // 设置为 true 以启用 cmd_vel 发布，设置为 false 以禁用发布
            if (cmd_vel_client_.call(srv)) {
                ROS_INFO("Service call succeeded if(extra_string == charging || !isSuspend_) false 发布速度停止指令给movebase: %s", srv.response.message.c_str());
            } else {
                ROS_ERROR("Failed to call service if(extra_string == charging || !isSuspend_) false  toggle_cmd_vel  发布速度停止指令给movebase ");
            }

            // ROS_INFO("可以绕过障碍物 或回充电桩 if(extra_string != ) ..extra_string：   %s",extra_string.c_str());

            //  ROS_INFO("可以绕过障碍物。或回充电桩当前状态state_machine_.get_current_state(): %s", state_machine_.get_current_state().c_str() );
            return;
        }

        //发布速度
        geometry_msgs::Twist cmd_vel;
        cmd_vel.linear.x = 0;
        cmd_vel.linear.y = 0;
        cmd_vel.linear.z = 0;
        cmd_vel.angular.x = 0;
        cmd_vel.angular.y = 0;
        cmd_vel.angular.z = 0;

        //获取小车当前位置
        geometry_msgs::PoseStamped current_pose;
        if(!getRobotPose(current_pose)){
            ROS_ERROR("获取小车当前位置 my_global_planner cannot make a plan for you because it could not get the start pose of the robot");
            //日志
            //  contnav_error(__FILE__,__LINE__,const_cast<char*>(response.message.c_str()));
            //  contnav_error(__FILE__,__LINE__,"global_planner::goalCB 获取小车当前位置 error");
            return ;
        }


        boost::shared_ptr<nav_msgs::Path  const>    local_path = ros::topic::waitForMessage<nav_msgs::Path>(local_plan_toptic_, ros::Duration(5));
        if(local_path){
            static const unsigned char INSCRIBED_INFLATED_OBSTACLE = 253;
            static const unsigned char FREE_SPACE = 0;
            //判断障碍物
            double min_distance_to_obstacle = std::numeric_limits<double>::max();
            for (const auto& pose_stamped : local_path->poses) {
                unsigned int mx, my;

                // Convert the pose in the path to map coordinates 将路径中的姿势转换为图坐标
                origin_costmap_->worldToMap(pose_stamped.pose.position.x, pose_stamped.pose.position.y, mx, my);

                // Check if the cell in the costmap is an obstacle 检查成本图中的单元格是否为障碍物// INSCRIBED_INFLATED_OBSTACLE = 253;
                if (origin_costmap_->getCost(mx, my) >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE) {



                    double distance_to_obstacle = sqrt(pow(current_pose.pose.position.x - pose_stamped.pose.position.x, 2) +
                                                       pow(current_pose.pose.position.y - pose_stamped.pose.position.y, 2));

                    if (distance_to_obstacle < min_distance_to_obstacle) {
                        min_distance_to_obstacle = distance_to_obstacle;// 机器人到障碍物的最小距离
                    }
                }
            }
            // Use the min_distance_to_obstacle to make decisions, such as stopping and waiting, or replanning //使用min_dinstance_to_obtacle做出决定，例如停止并等待，或重新规划
            if(min_distance_to_obstacle < min_distance_to_obstacle_) {//// 机器人到障碍物的最小距离 障碍物距离阀值；小于(5*0.05)米
                //--------------------------------------------------------------------------
                //停止并等待，发送速度0
                for (int i = 0; i < 10; ++i) {
                    cmd_vel_pub_.publish(cmd_vel);
                    // ros::Duration(0.001).sleep();
                }
                stopCmdFig_= true;//发布速度停止指令
                srv.request.data = true;  // 设置为 true 以启用 cmd_vel 发布，设置为 false 以禁用发布
                if (cmd_vel_client_.call(srv)) {
                    ROS_INFO("1 Service call succeeded 发布速度停止指令给movebase: %s", srv.response.message.c_str());
                } else {
                    ROS_ERROR("Failed to call service toggle_cmd_vel  发布速度停止指令给movebase ");
                }
                //--------------------------------------------------------------------------
                // 如果障碍物刚被检测到，记录当前时间
                if (obstacle_detected_time_.isZero())
                {
                    obstacle_detected_time_ = ros::Time::now();
                }
                // 计算已经等待了多长时间
                double time_waited = (ros::Time::now() - obstacle_detected_time_).toSec();
                // 如果等待时间大于阈值，尝试绕过障碍物
                if (time_waited > wait_time_)
                {
                    ROS_ERROR("1 取消上一次任务 progress.state_machine_.get_current_state(): %s", state_machine_.get_current_state().c_str() );
                    //绕过障碍物，遍历剩余路径，取出不在路径上的点位，然后规划路径到目标点，再规划路径；
                    //state_machine_.set_state("monitor_progress", {start_pose,target_pose,target_pose_default, std::string("AllowToBypassObstacles")});///AllowToBypassObstacles 许绕过障碍
                    //或者原路返回

//                    geometry_msgs::PoseStamped start_pose = boost::any_cast<geometry_msgs::PoseStamped>(args[0]);
//                    geometry_msgs::PoseStamped target_pose = boost::any_cast<geometry_msgs::PoseStamped>(args[1]);
//                    geometry_msgs::PoseStamped target_pose_default = boost::any_cast<geometry_msgs::PoseStamped>(args[2]);//默认点位，可以从数据库中读取，可以回充电装


                    geometry_msgs::PoseStamped start_pose, target_pose, target_pose_default;
                    std::string extra_string="EMPTY";

                    try
                    {
                        start_pose = any_cast<geometry_msgs::PoseStamped>(args[0]);
                        target_pose = any_cast<geometry_msgs::PoseStamped>(args[1]);
                        target_pose_default = any_cast<geometry_msgs::PoseStamped>(args[2]);
                        extra_string = any_cast<std::string>(args[3]);
                    }
                    catch(...)
                    {
                        // handle the exception
                        ROS_INFO("3.获取状态机数据monitor_progress 失败" );
                        return ;
                    }

                    ROS_INFO("2 取消上一次任务 progress.state_machine_.get_current_state(): %s", state_machine_.get_current_state().c_str() );

                    ROS_INFO("2 取消上一次任务 extra_string: %s", extra_string.c_str() );

                    //todo 绕障碍或者回桩


                    if(isReturnPile_&&extra_string!="navigate_to_goalpath_goalCB"){//false绕障碍,rue回桩, goalCB这个是代表rviz发出的信号，代表没有默认点位不能主回充
                        // 超过阈值，尝试绕过障碍物，重置检测到障碍物的时间
                        obstacle_detected_time_ = ros::Time(0);
                        isSuspend_=true; //true:暂停,false 绕障碍
                        //取消上一次任务
                        ac_->cancelGoal();
                        ROS_INFO("3 取消上一次任务 progress.state_machine_.get_current_state(): %s", state_machine_.get_current_state().c_str() );
                        //找到这条路径的id，取出路径，然后反向运动到这条路径的起点，然后重新规划路径到目标点（可以是充电桩充电）
                        int state_params_size= state_machine_.set_state("navigate_to_goalpathDefaultgoal", {start_pose,target_pose, target_pose_default,std::string("charging")});///charging 充电
                        ROS_INFO("4 取消上一次任务 navigate_to_goalpathDefaultgoal  获取翻转路径: %s", state_machine_.get_current_state().c_str() );
                        cout<<"======int state_params_size======navigate_to_goalpathDefaultgoal======state_params_size: "<<state_params_size<<endl;
                        stopCmdFig_= false;//发布速度停止指令
                        srv.request.data = false;  // 设置为 true 以启用 cmd_vel 发布，设置为 false 以禁用发布
                        if (cmd_vel_client_.call(srv)) {
                            ROS_INFO("2 Service call succeeded 发布速度停止指令给movebase: %s", srv.response.message.c_str());
                        } else {
                            ROS_ERROR("Failed to call service toggle_cmd_vel  发布速度停止指令给movebase ");
                        }


                    }else if(!isReturnPile_){
                        isSuspend_= false; //true:暂停,false 绕障碍
                        cout<<"*****************************======绕障碍=============***************************************888"<<endl;

                    }else{
                        cout<<"start*****************************======通过rviz控制的 ，取消上一次任务 ，程序结束，（不做回桩，不做绕障碍）==================================:isReturnPile_： "<<(isReturnPile_?"true":"fasle")<<endl;
                        state_machine_.set_state("idle", {start_pose,target_pose,target_pose_default,std::string("EMPTY")});//闲置状态,初始化给一个控制
                        //取消上一次任务
                        ac_->cancelGoal();
                        // 超过阈值，尝试绕过障碍物，重置检测到障碍物的时间
                        obstacle_detected_time_ = ros::Time(0);
                        stopCmdFig_= false;//发布速度停止指令
                        srv.request.data = false;  // 设置为 true 以启用 cmd_vel 发布，设置为 false 以禁用发布
                        isSuspend_= true; //true:暂停,false 绕障碍
                        if (cmd_vel_client_.call(srv)) {
                            ROS_INFO("3 Service call succeeded 发布速度停止指令给movebase: %s", srv.response.message.c_str());
                        } else {
                            ROS_ERROR("Failed to call service toggle_cmd_vel  发布速度停止指令给movebase ");
                        }
                        cout<<"end*****************************======通过rviz控制的 ，什么都不做，程序结束==================================isReturnPile_:"<<(isReturnPile_?"true":"false")<<endl;


                    }




                    //todo
                    return;

                }


                ROS_INFO("=============================停止并等待，前方有障碍物=================min_distance_to_obstacle======%s",to_string(min_distance_to_obstacle).c_str());
                ROS_INFO("=============================停止并等待，前方有障碍物================min_distance_to_obstacle_=======%s",to_string(min_distance_to_obstacle_).c_str());
                ROS_INFO("=============================停止并等待，前方有障碍物================等待时间 time_waited=======%s",to_string(time_waited).c_str());
                ROS_INFO("=============================停止并等待，前方有障碍物================等待时间阀值 wait_time_=======%s",to_string(wait_time_).c_str());
            }


//            else{
//                geometry_msgs::PoseStamped start_pose, target_pose, target_pose_default;
//                std::string current_string;
//                boost::any age0,age1,age2,age3;
//                state_machine_.get_current_state_param(0,age0);//起始点
//                state_machine_.get_current_state_param(1,age1);//目标点
//                state_machine_.get_current_state_param(2,age2);//默认点
//                state_machine_.get_current_state_param(3,age3);//获取当前状态的参数
//                std::string  current_state = state_machine_.get_current_state();//当前状态
//                try
//                {
//                    start_pose = any_cast<geometry_msgs::PoseStamped>(age0);
//                    target_pose = any_cast<geometry_msgs::PoseStamped>(age1);
//                    target_pose_default = any_cast<geometry_msgs::PoseStamped>(age2);
//                    current_string = any_cast<std::string>(age3);
//                    if(current_state=="monitor_progress"){
//                        if(current_string=="charging"){
//                            int state_params_size= state_machine_.set_state("navigate_to_goalpathDefaultgoal", {start_pose,target_pose, target_pose_default,current_string});///charging 充电
//                            ROS_INFO("if(local_path)else : 重新发布任务  navigate_to_goalpathDefaultgoal  获取翻转路径: %s", state_machine_.get_current_state().c_str() );
//
//                            //todo
//
//                        }else if(current_string=="navigate_to_goalpath"||current_string=="navigate_to_goalpath_goalCB"){
//                            int state_params_size=state_machine_.set_state("navigate_to_goalPath", {start_pose,target_pose, target_pose_default,current_string});// target_pose_default_;//返回默认点位，或是充电点位
//                            ROS_INFO("if(local_path)else : 重新发布任务  navigate_to_goalpath  获取路径: %s", state_machine_.get_current_state().c_str() );
//
//                            //todo
//
//                        }else if(current_string=="navigate_to_goalpathAgain"){
//                            int state_params_size=state_machine_.set_state("navigate_to_goalpathAgain", {start_pose,target_pose, target_pose_default,current_string});// target_pose_default_;//返回默认点位，或是充电点位
//                            ROS_INFO("if(local_path)else : 重新发布任务  navigate_to_goalpathAgain  获取路径: %s", state_machine_.get_current_state().c_str() );
//
//                        }else if(current_string=="navigate_to_goalpathOverturn"){
//                            int state_params_size=state_machine_.set_state("navigate_to_goalpathOverturn", {start_pose,target_pose, target_pose_default,current_string});// target_pose_default_;//返回默认点位，或是充电点位
//                            ROS_INFO("if(local_path)else : 重新发布任务  navigate_to_goalpathOverturn  获取路径: %s", state_machine_.get_current_state().c_str() );
//
//                        }else if(current_string=="navigate_to_goalpathOverturnAgain"){
//                            int state_params_size=state_machine_.set_state("navigate_to_goalpathOverturnAgain", {start_pose,target_pose, target_pose_default,current_string});// target_pose_default_;//返回默认点位，或是充电点位
//                            ROS_INFO("if(local_path)else : 重新发布任务  navigate_to_goalpathOverturnAgain  获取路径: %s", state_machine_.get_current_state().c_str() );
//
//                        }
//                        restore();//恢复
//
//                        return;
//                    }// if(current_state=="monitor_progress")
//
//                }
//                catch(...)
//                {
//                    geometry_msgs::PoseStamped start_pose,target_pose,target_pose_default;
//                    // int state_params_size=state_machine_.set_state("error", {start_pose,target_pose,target_pose_default,std::string("EMPTY")});//错误状态
//                    int state_params_size=state_machine_.set_state("idle", {start_pose,target_pose, target_pose_default,std::string("EMPTY")});// target_pose_default_;//返回默认点位，或是充电点位
//                    restore();//恢复
//                    ROS_ERROR(" monitor_progress 获取数据 state_machine_.get_current_state_param 中的数据类型转换失败 ");
//                    cout<<"t======int state_params_size===monitor_progress=========error: "<<state_params_size<<endl;
//                    return;
//
//                }
//
//
//            }//if(min_distance_to_obstacle < min_distance_to_obstacle_)

        }else{
            for (int i = 0; i < 10; ++i) {
                cmd_vel_pub_.publish(cmd_vel);
                // Sleep for 1 second
                //ros::Duration(0.001).sleep();
            }


            // 如果障碍物刚被检测到，记录当前时间
//            if (obstacle_locaplanner_time_.isZero())
//            {
//                obstacle_locaplanner_time_ = ros::Time::now();
//            }
//            // 计算已经等待了多长时间
//            double time_waited = (ros::Time::now() - obstacle_locaplanner_time_).toSec();
//            // 如果等待时间大于阈值，尝试绕过障碍物
//            if (time_waited > locaplanner_wait_time_)
//            {

                geometry_msgs::PoseStamped start_pose, target_pose, target_pose_default;
                std::string current_string;
                boost::any age0,age1,age2,age3;
                state_machine_.get_current_state_param(0,age0);//起始点
                state_machine_.get_current_state_param(1,age1);//目标点
                state_machine_.get_current_state_param(2,age2);//默认点
                state_machine_.get_current_state_param(3,age3);//获取当前状态的参数
                std::string  current_state = state_machine_.get_current_state();//当前状态
                try
                {
                    start_pose = any_cast<geometry_msgs::PoseStamped>(age0);
                    target_pose = any_cast<geometry_msgs::PoseStamped>(age1);
                    target_pose_default = any_cast<geometry_msgs::PoseStamped>(age2);
                    current_string = any_cast<std::string>(age3);
                    if(current_state=="monitor_progress"){
                        if(current_string=="charging"){
                            int state_params_size= state_machine_.set_state("navigate_to_goalpathDefaultgoal", {start_pose,target_pose, target_pose_default,current_string});///charging 充电
                            ROS_INFO("if(local_path)else : 重新发布任务  navigate_to_goalpathDefaultgoal  获取翻转路径: %s", state_machine_.get_current_state().c_str() );

                            //todo

                        }else if(current_string=="navigate_to_goalpath"||current_string=="navigate_to_goalpath_goalCB"){
                            int state_params_size=state_machine_.set_state("navigate_to_goalPath", {start_pose,target_pose, target_pose_default,current_string});// target_pose_default_;//返回默认点位，或是充电点位
                            ROS_INFO("if(local_path)else : 重新发布任务  navigate_to_goalpath  获取路径: %s", state_machine_.get_current_state().c_str() );

                            //todo

                        }else if(current_string=="navigate_to_goalpathAgain"){
                            int state_params_size=state_machine_.set_state("navigate_to_goalpathAgain", {start_pose,target_pose, target_pose_default,current_string});// target_pose_default_;//返回默认点位，或是充电点位
                            ROS_INFO("if(local_path)else : 重新发布任务  navigate_to_goalpathAgain  获取路径: %s", state_machine_.get_current_state().c_str() );

                        }else if(current_string=="navigate_to_goalpathOverturn"){
                            int state_params_size=state_machine_.set_state("navigate_to_goalpathOverturn", {start_pose,target_pose, target_pose_default,current_string});// target_pose_default_;//返回默认点位，或是充电点位
                            ROS_INFO("if(local_path)else : 重新发布任务  navigate_to_goalpathOverturn  获取路径: %s", state_machine_.get_current_state().c_str() );

                        }else if(current_string=="navigate_to_goalpathOverturnAgain"){
                            int state_params_size=state_machine_.set_state("navigate_to_goalpathOverturnAgain", {start_pose,target_pose, target_pose_default,current_string});// target_pose_default_;//返回默认点位，或是充电点位
                            ROS_INFO("if(local_path)else : 重新发布任务  navigate_to_goalpathOverturnAgain  获取路径: %s", state_machine_.get_current_state().c_str() );

                        }
                        restore();//恢复

                        return;
                    }// if(current_state=="monitor_progress")

                }
                catch(...)
                {
                    geometry_msgs::PoseStamped start_pose,target_pose,target_pose_default;
                    // int state_params_size=state_machine_.set_state("error", {start_pose,target_pose,target_pose_default,std::string("EMPTY")});//错误状态
                    int state_params_size=state_machine_.set_state("idle", {start_pose,target_pose, target_pose_default,std::string("EMPTY")});// target_pose_default_;//返回默认点位，或是充电点位
                    restore();//恢复
                    ROS_ERROR(" monitor_progress 获取数据 state_machine_.get_current_state_param 中的数据类型转换失败 ");
                    cout<<"t======int state_params_size===monitor_progress=========error: "<<state_params_size<<endl;
                    return;

                }


           // }// if (time_waited > wait_time_)




        }//if(local_path)

    }
    //恢复速度
    void moveAction::restore(){
                //恢复代价地图
                std_srvs::Empty srv1;
                clear_costmaps_client_.call(srv1);

                 std_srvs::SetBool srv;
               // 超过阈值，尝试绕过障碍物，重置检测到障碍物的时间
                obstacle_detected_time_ = ros::Time(0);
                stopCmdFig_= false;//发布速度停止指令
                isSuspend_= true; //true:暂停,false 绕障碍
                srv.request.data = false;  // 设置为 true 以启用 cmd_vel 发布，设置为 false 以禁用发布
                if (cmd_vel_client_.call(srv)) {
                    ROS_INFO("6 restore Service call succeeded 发布速度停止指令给movebase: %s", srv.response.message.c_str());
                } else {
                    ROS_ERROR("restore Failed to call service toggle_cmd_vel  发布速度停止指令给movebase ");
                }
    }

    //恢复速度
    void moveAction::restore2(){
        //恢复代价地图
        std_srvs::Empty srv1;
        clear_costmaps_client_.call(srv1);

        std_srvs::SetBool srv;
        stopCmdFig_= false;//发布速度停止指令
        isSuspend_= true; //true:暂停,false 绕障碍
        srv.request.data = false;  // 设置为 true 以启用 cmd_vel 发布，设置为 false 以禁用发布
        if (cmd_vel_client_.call(srv)) {
            ROS_INFO("6 restore Service call succeeded 发布速度停止指令给movebase: %s", srv.response.message.c_str());
        } else {
            ROS_ERROR("restore Failed to call service toggle_cmd_vel  发布速度停止指令给movebase ");
        }
    }

    void moveAction::init() {
        restore();
        //初始化给一个控制
        geometry_msgs::PoseStamped start_pose,target_pose,target_pose_default;
        state_machine_.set_state("idle", {start_pose,target_pose,target_pose_default,std::string("EMPTY")});//闲置状态,初始化给一个控制



       // mbf_msgs::ExePathGoal global_path;//未走过的路径
      //  mbf_msgs::ExePathGoal  traversed_path;//添加到已走过的路径中
       // global_path_=global_path;//清除全局路径
       // traversed_path_=traversed_path;//清除已经走过的路径



    }


    //获取数据 boost::any 中的数据，函数的输入参数包括 boost::any 对象和其期望的类型，函数的返回值是类型转换后的值
    template<typename T> T moveAction::any_cast(const boost::any & operand)
    {
        try
        {
            return boost::any_cast<T>(operand);
        }
        catch (boost::bad_any_cast &)
        {
            ROS_ERROR("获取数据 boost::any 中的数据类型转换失败 ,Failed to cast %s to %s", operand.type().name(), typeid(T).name());
            throw;
        }
    }

   //-------------------------------------------------------


    //订阅一个全局大地图，做障碍和珊格地图的提取
    void moveAction::grid_callback(nav_msgs::OccupancyGridConstPtr const& msg) {
        std::cout << "*******1.moveAction 订阅一个全局大地图，做障碍和珊格地图的提取 frame_id_：****** " <<frame_id_<<std::endl;
        grid_ready_ = false;
        grid_ = *msg;
        frame_id_ = msg->header.frame_id;
        std::cout << "1.moveAction 订阅一个全局大地图，做障碍和珊格地图的提取 frame_id_： " <<frame_id_<<std::endl;
        // 格栅地图转换为代价地图
        origin_costmap_ = grid2costmap(grid_);
        cx_ = origin_costmap_->getSizeInCellsX(), cy_ = origin_costmap_->getSizeInCellsY();
        setSize( cx_,  cy_);
        grid_ready_ = true;

    }

    void moveAction::grid_updated_callback(map_msgs::OccupancyGridUpdateConstPtr const& msg) {
        if (!grid_ready_) return;
       // std::cout << "*******2.grid_updated_callback 更新一个全局大地图，做障碍和珊格地图的提取 frame_id_：****** " <<frame_id_<<std::endl;

        grid_updated_ready_ = false;
        int index = 0;
        for (unsigned y = msg->y; y < msg->y + msg->height; ++y) {
            for (unsigned x = msg->x; x < msg->x + msg->width; ++x) {
                grid_.data[y * grid_.info.width + x] = msg->data[index++];
            }
        }
        // 格栅地图转换为代价地图
        origin_costmap_ = grid2costmap(grid_);
        cx_ = origin_costmap_->getSizeInCellsX(), cy_ = origin_costmap_->getSizeInCellsY();
        setSize( cx_,  cy_);
        grid_updated_ready_ = true;
    }


    /**
可选参数：

mode: Trinary是默认值三元解释，即解释所有值，使输出最终成为三个值之一

灰度值 [0,255] 转换成p值 [0,1]

转换公式 p=(255-x)/255.0

---

        4.map与costmap都是以nav_msgs::OccupancyGrid类型发布其topic。其中整张地图的障碍物信息存放在data数据成员中，data是一个int8类型的vector向量，即一维数组。假设一张pgm的map地图，宽：width，高：height，单位为像素，分辨路为resolution，左下角像素点在世界坐标系下的位置为：（ox,oy）,单位米，那么世界坐标系下一点（x,y）单位米，假设其在地图中，那么该点对应的data中的索引index为：
        index = (x-ox)/resolution+(y-oy)/resolution*width
        （注：index为int8类型，所以要将结果进行下类型转换），

        那么该点在地图中的信息即为data[index]
        即：data是按照那张地图图片的自底向上，自左至右逐个像素点存储的
        map中data存储的格式如下：
        0:空白区域
        100:障碍物
        -1:未知
        1-99:根据像素的灰度值转换后的值，代表有障碍物的概率（不确定，猜测是这个含义，实际用的时候把这个值都当作无障碍物）
        costmap中data存储格式如下：
        0:空白区域
        100:障碍物层
        99:膨胀层
        1-98:距离障碍物层的远近获得的代价值
        -1:未知区域


        打印nav_msgs::OccupancyGrid发现，data中只存有0,100，-1，三种信息，没有1-99
        ————————————————
        版权声明：本文为CSDN博主「再遇当年」的原创文章，遵循CC 4.0 BY-SA版权协议，转载请附上原文出处链接及本声明。
        原文链接：https://blog.csdn.net/qq_15204179/article/details/121395464
     */


//格栅地图转换为代价地图    //https://blog.csdn.net/qq_35635374/article/details/120918847 二维占据栅格地图costmap生成方法及源码解析
    auto
    moveAction::grid2costmap(nav_msgs::OccupancyGrid const& grid) const -> std::shared_ptr<costmap_2d::Costmap2D> {
        auto costmap = std::make_shared<costmap_2d::Costmap2D>(grid.info.width,
                                                               grid.info.height,
                                                               grid.info.resolution,
                                                               grid.info.origin.position.x,
                                                               grid.info.origin.position.y);
        costmap->setDefaultValue(255);

        costmap->resetMap(0, 0, costmap->getSizeInCellsX(), costmap->getSizeInCellsY());
        for (unsigned y = 0; y < grid.info.height; y++) {
            for (unsigned x = 0; x < grid.info.width; x++) {
                if (grid.data[y * grid.info.width + x] < 0) {
                    /**
                    static const unsigned char NO_INFORMATION = 255;
                    static const unsigned char LETHAL_OBSTACLE = 254;
                    static const unsigned char INSCRIBED_INFLATED_OBSTACLE = 253;
                    static const unsigned char FREE_SPACE = 0;
                    */
                    costmap->setCost(x, y, costmap_2d::NO_INFORMATION);//255
                    continue;
                }



                //   100:障碍物层     99:膨胀层    1-98:距离障碍物层的远近获得的代价值
                if (grid.data[y * grid.info.width + x] >98&& grid.data[y * grid.info.width + x] <=100) {//苏凯 自己加的代码
                    /**
                    static const unsigned char NO_INFORMATION = 255;
                    static const unsigned char LETHAL_OBSTACLE = 254;
                    static const unsigned char INSCRIBED_INFLATED_OBSTACLE = 253;
                    static const unsigned char FREE_SPACE = 0;
                    */
                    costmap->setCost(x, y, costmap_2d::LETHAL_OBSTACLE);//254
                    continue;
                }

                /**
                1-99:根据像素的灰度值转换后的值，代表有障碍物的概率（不确定，猜测是这个含义，实际用的时候把这个值都当作无障碍物）
                costmap中data存储格式如下：
                0:空白区域
                100:障碍物层
                99:膨胀层
                1-98:距离障碍物层的远近获得的代价值
                -1:未知区域
                */
                costmap->setCost(x, y, static_cast<uint8_t>(grid.data[y * grid.info.width + x] * 254 / 100));
            }
        }

        return costmap;
    }
//代价地图转换为cv::Mat
/**
    auto moveAction::costmap2cv_mat(std::shared_ptr<costmap_2d::Costmap2D> const& costmap,
                                             int32_t x,
                                             int32_t y,
                                             int32_t width,
                                             int32_t height) -> cv::Mat {
        auto ret_mat = cv::Mat(height, width, CV_8U);
        for (auto r = 0; r < height; r++) {
            for (auto c = 0; c < width; c++) {
                //costmap's origin is at left bottom ,while opencv's pic's origin is at left-top.
                auto mx = c + x;
                auto my = y + height - r - 1;
                if (mx < 0 || mx >= costmap->getSizeInCellsX() || my < 0 || my >= costmap->getSizeInCellsY()) {
                    continue;
                }

                ret_mat.at<uchar>(r, c)
                        = costmap->getCost(static_cast<unsigned>(mx), static_cast<unsigned>(my));
            }
        }

        return ret_mat;
    }
*/
//----------------------------------------------------


    bool moveAction::getuwb_tf(string &pframe_id,string &cframe_id,geometry_msgs::TransformStamped &ps_out) {

        try{


            ps_out = buffer2.lookupTransform(pframe_id, cframe_id, ros::Time(0),ros::Duration(2));
//            ROS_INFO("相对关系信息:父级：%s,子级：%s 偏移量(%.2f,%.2f,%.2f) 4元素(qx %.5f,qy  %.5f,qz %.5f,qw  %.5f)",
//                     ps_out.header.frame_id.c_str(), // son2
//                     ps_out.child_frame_id.c_str(),  // son1
//                     ps_out.transform.translation.x,
//                     ps_out.transform.translation.y,
//                     ps_out.transform.translation.z,
//                     ps_out.transform.rotation.x,
//                     ps_out.transform.rotation.y,
//                     ps_out.transform.rotation.z,
//                     ps_out.transform.rotation.w
//            );
        }catch (exception e){
            std::cout << "=======getuwb_urdf=====异常===================" << std::endl;
            return false;
        }

        return true;
    }



//================================================
    //清除成本图中的起始单元格，因为我们知道它不会成为障碍
    void moveAction::clearRobotCell(const geometry_msgs::PoseStamped& global_pose, unsigned int mx, unsigned int my) {

        //set the associated costs in the cost map to be free
        origin_costmap_->setCost(mx, my, costmap_2d::FREE_SPACE);//自由空间
    }


    /**
 * 具体而言，该函数的实现方式是，分别用指针 pc 指向数组的第一行、最后一行、第一列和最后一列，
 * 然后使用循环依次遍历这四个边界，并将它们的每个元素设置为指定的 value 值。

在代码的实现过程中，nx 和 ny 分别表示数组 costarr 的宽度和高度，unsigned char 是一个无符号字符类型，用
 于存储 8 位无符号整数。因此，该函数的输入参数包括一个指向无符号字符数组的指针 costarr，数组的宽度 nx 和高度 ny，以及要设置的值 value。
 * @param costarr
 * @param nx
 * @param ny
 * @param value
 */
    void moveAction::outlineMap(unsigned char* costarr, int nx, int ny, unsigned char value) {
        // 首先，将数组第一行全部设置为指定的值
        unsigned char* pc = costarr;
        for (int i = 0; i < nx; i++)
            *pc++ = value;
        // 接着，将数组最后一行全部设置为指定的值
        pc = costarr + (ny - 1) * nx;
        for (int i = 0; i < nx; i++)
            *pc++ = value;
        //  // 然后，将数组第一列全部设置为指定的值
        pc = costarr;
        for (int i = 0; i < ny; i++, pc += nx)
            *pc = value;

        // // 最后，将数组最后一列全部设置为指定的值
        pc = costarr + nx - 1;
        for (int i = 0; i < ny; i++, pc += nx)
            *pc = value;

        /**
       pc = costarr + (ny - 1) * nx; 解释一下
       这行代码的作用是让指针 pc 指向数组 costarr 的最后一行的第一个元素。具体而言，由于 costarr 是一个一维数组，按照行优先的顺序存储，因此最后一行的第一个元素的下标应该是 (ny - 1) * nx。
       这里用指针的算术运算 costarr + (ny - 1) * nx 来获得该元素的地址，并将其赋值给指针 pc。这样，接下来就可以通过对 pc 进行递增操作，遍历数组 costarr 的最后一行了。
       */
    }
    //计算起点在一维数组中的下标。// 将起点坐标 start_x 和 start_y 转化为一维数组中的索引 start_i。
    int moveAction::toIndex(int x, int y,int nx ,int ny) {
        return x + nx * y;
    }

    //某个位置在字符数组中的索引为 index，则该位置在地图中的 current_x 和 current_y 像素坐标可以通过以下公式计算：
    void moveAction::toX_Y(int index,int &current_x, int &current_y,int nx ,int ny) {
        current_x = index % nx;
        current_y = index / nx;
    }
    void moveAction::setSize(int nx, int ny) {
        nx_ = nx;
        ny_ = ny;
        ns_ = nx * ny;
    } /**< sets or resets the size of the map */
//获取小车当前位置
    bool moveAction::getRobotPose(geometry_msgs::PoseStamped& global_pose)
    {
        bool  getuwb_tfmapTbase_linkFig=false;
        try {
            //获取矩阵
            geometry_msgs::TransformStamped mapTbase_link;
            string pframe_id ="map";//基坐标
            string cframe_id ="base_link";//base_footprint  base_link
            getuwb_tfmapTbase_linkFig = getuwb_tf(pframe_id,cframe_id,mapTbase_link);

            if(getuwb_tfmapTbase_linkFig){

                global_pose.header.frame_id = mapTbase_link.header.frame_id;
                global_pose.header.stamp = mapTbase_link.header.stamp;
                global_pose.pose.position.x = mapTbase_link.transform.translation.x;
                global_pose.pose.position.y = mapTbase_link.transform.translation.y;
                global_pose.pose.position.z = mapTbase_link.transform.translation.z;
                global_pose.pose.orientation.x = mapTbase_link.transform.rotation.x;
                global_pose.pose.orientation.y = mapTbase_link.transform.rotation.y;
                global_pose.pose.orientation.z = mapTbase_link.transform.rotation.z;
                global_pose.pose.orientation.w = mapTbase_link.transform.rotation.w;
                //std::cout << "=======getRobotPose====获取到机器人当前坐标===================" << std::endl;
                return true;
            } else{
                std::cout << "=======getRobotPose====error 未获取到机器人当前坐标===================" << std::endl;
                return false;
            }

        }catch (exception e){
            std::cout << "=======getRobotPose====异常 未获取到机器人当前坐标===================" << std::endl;
            return false;
        }

    }




/**
    std::map<std::string, int> myMap;
// 添加键值对到 myMap

    std::string keyToCheck = "example_key";
    if (containsKey(myMap, keyToCheck)) {
    // 键存在
} else {
// 键不存在
}
*/

//判断map判断key是否存在
    template<typename KeyType, typename ValueType>
    bool moveAction::containsKey(const std::map<KeyType, ValueType>& myMap, const KeyType& keyToCheck) {
        return myMap.find(keyToCheck) != myMap.end();
    }


    //读取文件
    std::vector<geometry_msgs::PoseStamped> moveAction::readEulerAnglesFromFile(const std::string& filename)
    {
        std::vector<geometry_msgs::PoseStamped> plan;
        std::ifstream infile(filename.c_str());
        if (!infile.is_open())
        {
            std::cerr << "Failed to open file: " << filename << std::endl;
            return plan;
        }

        geometry_msgs::PoseStamped pose;
        while (infile >> pose.pose.position.x >> pose.pose.position.y >> pose.pose.position.z)
        {
            double yaw;
            infile >> yaw;

            tf2::Quaternion quat;
            quat.setRPY(0.0, 0.0, yaw);

            pose.pose.orientation = tf2::toMsg(quat);
            plan.push_back(pose);
        }

        infile.close();

        return plan;
    }

    //写入文件
    void moveAction::writeEulerAnglesToFile(const std::vector<geometry_msgs::PoseStamped>& plan, const std::string& filename)
    {
        std::ofstream outfile;
        outfile.open(filename.c_str());

        if (!outfile.is_open())
        {
            std::cerr << "写入文件 Failed to open file: " << filename << std::endl;
            return;
        }

        for (const auto& pose : plan)
        {
            tf2::Quaternion quat;
            tf2::fromMsg(pose.pose.orientation, quat);

            double roll, pitch, yaw;
            tf2::Matrix3x3(quat).getRPY(roll, pitch, yaw);

            outfile << pose.pose.position.x << " " << pose.pose.position.y << " "
                    << pose.pose.position.z << " " << yaw << std::endl;
        }

        outfile.close();

        std::cout << "写入文件 Euler angles have been saved to " << filename << std::endl;
    }
    //创建文件路径
    void moveAction::createDirectory(const std::string& path)
    {
        struct stat info;

        if (stat(path.c_str(), &info) != 0)
        {
            // 如果路径不存在，则创建路径
            int status = mkdir(path.c_str(), S_IRWXU | S_IRWXG | S_IROTH | S_IXOTH);

            if (status != 0)
            {
                std::cerr << "如果路径不存在，则创建路径 Failed to create directory: " << path << std::endl;
            }
        }
    }









     //像素坐标转世界坐标
    void moveAction::mapToWorld(double mx, double my, double& wx, double& wy) {
        wx = origin_costmap_->getOriginX() + mx * origin_costmap_->getResolution();
        wy = origin_costmap_->getOriginY() + my * origin_costmap_->getResolution();
    }

/**
 *
 * @param wx
 * @param wy
 * @param mx
 * @param my
 这个函数的实现比较简单，主要包括以下几个步骤：

    检查给定的世界坐标 (wx, wy) 是否在 Costmap2D 对象覆盖的区域内，如果不在，则返回 false。

    将给定的世界坐标 (wx, wy) 转换为相应的离散地图坐标 (mx, my)。

    检查转换后的地图坐标 (mx, my) 是否在 Costmap2D 对象的有效范围内，如果超出，则返回 false。

    如果以上检查都通过了，则返回 true。

下面是对这个函数中各个参数的说明：

    wx 和 wy：待转换的世界坐标。
    mx 和 my：转换后的离散地图坐标。像素坐标
    origin_x_ 和 origin_y_：地图坐标系原点在世界坐标系中的位置。
    resolution_：地图的分辨率，即一个像素代表的实际距离。
    size_x_ 和 size_y_：地图的大小，即像素数。

需要注意的是，以上实现中使用了 C++ 中的类型转换语句 static_cast，将浮点数转换为整数类型，同时使用引用类型 & 将结果返回给调用者。
 */
//世界坐标转像素坐标
    bool moveAction::worldToMap(double wx, double wy, double& mx, double& my) {
        double origin_x = origin_costmap_->getOriginX(), origin_y = origin_costmap_->getOriginY();
        double resolution = origin_costmap_->getResolution();

        if (wx < origin_x || wy < origin_y)
            return false;

        /**
         * //参数：判断使用的是navfn包还是，old_navfn_behavior_这个参数默认为false，用来兼容navfn；然后调用clearEndpoint（）函数，该函数把终点周围的点更新了一下：
        //    old_navfn_behavior(default: false): 若在某些情况下,想让moveAction完全复制navfn的功能,那就设置为true,但是需要注意navfn是非常旧的ROS系统中使用的,现在已经都用moveAction代替navfn了,所以不建议设置为true.

            if(!old_navfn_behavior_)
                convert_offset_ = 0.5;//false
            else
                convert_offset_ = 0.0;//true
        */
        mx = (wx - origin_x) / resolution ;
        my = (wy - origin_y) / resolution ;
        //  costmap_->getSizeInCellsX() 和 getSizeInCellsY：地图的大小，即像素数。
        if (mx < origin_costmap_->getSizeInCellsX() && my < origin_costmap_->getSizeInCellsY())
            return true;

        return false;
    }



    //--namespace--
}


//=======================main================================

moveAction *currency = NULL;

//ctrl + c
void mySigintHandler(int sig) {
    std::cout << "ctrl c  sig: " << sig << std::endl;
    if (currency != NULL)
        delete currency;

    ros::shutdown();
}

// rosrun contnav_global_planner contnav_moveAction_node
int main(int argc, char **argv) {
    //main中init前面加
    setlocale(LC_ALL, "");  //中文
    ros::init(argc, argv, "contnav_moveAction_node");
    ros::NodeHandle nh;
    signal(SIGINT, mySigintHandler);
    tf2_ros::Buffer buffer(ros::Duration(30));
    tf2_ros::TransformListener tf(buffer);
    currency = new moveAction(buffer,nh);
    currency->run();

    return 0;
}

